Luận văn 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

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 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 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 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). 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. 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. 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 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 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 độ. 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. 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. 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. 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) 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. 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 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 = T5 0 [1,4] = ac1s234 L1c1c2 L2c1c23 L3c1c234 (3.1) y = T5 0 [2,4] = as1s234 L1c2s1 L2s1c23 L3s1c234 (3.2) z = T5 0 [3,4] = -ac234 L1s2 L2s23 L3s234 (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) 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 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 4 8 7 3Fmasat1 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) 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 59 01 20 72 22 3 38 6 152 51 R3 39 R1 27 140 º 140º Hình 3.15: Mô hình cánh tay robot 3D và không gian hoạt động. 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. 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: 140 0 θ2: 45 0 θ3: 90 0 600 θ4< 180 0 θ5: 360 0 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 29 Các ma trận chuyển đổi: T1 0 [ c1 0 s1 0 s1 0 c1 0 0 1 0 0 0 0 0 1 ] T2 1 [ c2 s2 0 L1.c2 s2 c2 0 L1.s2 0 0 1 0 0 0 0 1 ] T3 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 ] T5 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: T5 0 [ s1s5 c5c1c234 c5s1 s5c1c234 c1s234 px c1s5 c5s1c234 c1c5 s5s1c234 s1s234 py c5s234 s5s234 c234 pz 0 0 0 1 ] Trong đó: p x = ac1s234 L1c1c2 L2c1c23 L3c1c234 p y = as1s234 L1c2s1 L2s1c23 L3s1c234 p z = -ac234 L1s2 L2s23 L3s234 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 = T5 0 [1,4] = ac1s234 L1c1c2 L2c1c23 L3c1c234 (3.10) y = T5 0 [2,4] = as1s234 L1c2s1 L2s1c23 L3s1c234 (3.11) z = T5 0 [3,4] = -ac234 L1s2 L2s23 L3s234 (3.12) Trong đó: s234 sin ( 2 3 4) và c234 cos ( 2 3 4) 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: T1 0 1. T5 0 T5 1 T2 1 . T3 2 . T4 3 . T5 4 Dựa vào bài toán động học thuận ta có: VT : T1 0 -1 [ c1 s1 0 0 0 0 1 0 s1 -c1 0 0 0 0 0 1 ] ; T 5 0 [ T115 0 T125 0 T135 0 x T215 0 T225 0 T235 0 y T315 0 T325 0 T335 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ó: T5 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 ) 180 0 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-1i 1 i . Ta khảo xác phương trình ở khâu thứ 3. T 1. T 13 2 . T 12 1 . T 11 0 . T5 0 T5 4 4 3 VT: T-1. T-13 2 . T-12 1 4 3 [ c234 s234 0 -L1c34-L2c4-L3 0 0 1 0 s234 -c234 0 -L1s34-L2s4 0 0 0 1 ] 31 [ c1 s1 0 0 0 0 1 0 s1 -c1 0 0 0 0 0 1 ] ; [ T115 0 T125 0 T135 0 x T215 0 T225 0 T235 0 y T315 0 T325 0 T335 0 z 0 0 0 1] VP : T5 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. T135 0 s1. T235 0 ) + s234. T335 0 = 0 s234 c234 (c1. T135 0 s1. T235 0 ) T335 0 Suy ra: ( 2 3 4) arctan2 (-( ) T335 0 ) (3.14) và ( 2 3 4) arctan2 (-( ) T335 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ó: (c 234 (c1.x s1.y) s234.z L3) 2 (s234(c1.x s1.y) c234.z a) 2 c3 (c 234 (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: T1 0 1. T5 0 T5 1 T2 1 . T3 2 . T4 3 . T5 4 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)(L2c3 L1) (L3s3) 2 ( ) s2 s (z ac234 L3s234).(L1 L2c3) L2s3(c1.x s1.y as234 L3c234) (L1 L2c3)(L2c3 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) 180 0 234 = arctan(s23,c23) arctan(-(c1. T135 0 s1. T235 0 ), T335 0 ) 234 234 180 0 3 arctan(s3,c3) 33 với s3 √1-c3 2 c3 (c 234 (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)(L2c3 L1) (L3s3) 2 (z ac234 L3s234).(L1 L2c3) L2s3(c1.x s1.y as234 L3c234) (L1 L2c3)(L2c3 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 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. 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 node