Luận văn điện tử Điều khiển tay máy 3 bậc tự do dùng thị giác máy tính

Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh PHẦN A Võ Hoàng Trôvi – Nguyễn Sĩ Bích 1 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh Trong thời đại ngày nay, ngành công nghiệp đóng vai trò quan trọng trong nền kinh tế. Từ những ngành như sản xuất, chế biến lương thực thực phẩm, nước uống cho đến các ngành công nghệ chế tạo máy, công nghệ chế tạo ôtô, các ngành công nghệ cao v.v…Tất cả những ngành đó có được hiệu quả kinh tế cao không ít nhờ vào các hệ thống sản xuất tự động, những Robot tự động, những tay máy công nghiệp, chúng đã thay thế sức lao động con người một cách hiệu quả nhất. Robot được sử dụng rộng rãi từ những nơi mà môi trường có tính độc hại, nguy hiểm, độ chính xác cao trong công nghiệp cho đến các công việc hằng ngày. Do vậy Robot có tầm quan trọng rất lớn và là một trong những lĩnh vực nghiên cứu hàng đầu trong thời đại ngày nay. Là sinh viên chuyên ngành điện tử tự động, để bổ sung những kiến thức đã học cũng như nghiên cứu những vấn đề mới trong lĩnh vực điều khiển nên chúng em quyết định chọn đề tài “Điều khiển tay máy 3 bậc tự do dùng thị giác máy tính”. Thông qua luận văn chúng em sẽ khảo sát, nghiên cứu và điều khiển tay máy 3 bậc tự do để gắp vật cố định trên mặt phẳng nhờ sự trợ giúp bởi Camera xác định tọa độ vật. Để điều khiển tay máy có thể dùng vi xử lý, vi điều khiển hoặc PLC kết hợp với mạch công suất, mạch cảm biến Song thực tế cho thấy nếu xử lý ảnh dùng Camera số bằng vi điều khiển hoặc PLC thì rất khó thực hiện, tốc độ xử ảnh chậm và giá thành cao. Chúng em đã chọn phương pháp dùng máy tính để xử lý ảnh và điều khiển tay máy thông qua PLC. Mọi việc dễ dàng hơn nhiều và MATLAB trở thành phần mềm hữu hiệu để thực hiện theo cách trên. MATLAB tích hợp nhiều công cụ cho nhiều lĩnh vực trong đó có hai Toolbox cần thiết là: Image Processing Toolbox cho việc xử lý ảnh và OPC Toolbox cho việc kết nối PLC. Mặc dù đã cố gắng hoàn thành luận văn đúng thời hạn nhưng trong quá trình thực hiện không tránh khỏi những thiếu sót. Chúng em rất mong nhận được sự đóng góp ý kiến từ quý Thầy cô và các bạn để đề tài của chúng em ngày càng hoàn thiện hơn. Võ Hoàng Trôvi – Nguyễn Sĩ Bích 2 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh PHẦN B CHƯƠNG 1: CƠ SỞ LÝ THUYẾT Võ Hoàng Trôvi – Nguyễn Sĩ Bích 3 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh 1.1 Lý thuyết PLC S7-200 Lý thuyết PLC thì rất rộng nên trong chương này chúng em chỉ trình bày những khái niệm cơ bản và những phần lý thuyết liên quan PLC S7-200 loại CPU 224 được sử dụng trong luận văn. 1.1.1 Giới thiệu PLC S7-200: Trong hệ thống tự động thường gặp những thiết bị làm việc theo nguyên tắc tuần tự, tuân theo những quy luật được thiết kế sẵn với tín hiệu vào nằm hai mức có và không như contact, relay, mạch định thời. Các hệ thống này sẽ có sơ đồ phức tạp, độ tin cậy kém, đáp ứng không nhanh, dễ hỏng v.v…Từ những năm 70 để đáp ứng nhu cầu thực tế sản xuất, các mạch điều khiển bằng relay, bộ định thời đã không còn phù hợp, do đó bộ điều khiển logic khả trình (Programmable Logic Controller – PLC) ra đời. Các PLC đời đầu chỉ thực hiện các phép tính logic, tín hiệu vào là tín hiệu rời rạc. Hiện nay PLC còn thực hiện được các phép tính số học, logic và làm việc với tín hiệu vào là tín hiệu liên tục. PLC S7-200 là một trong những PLC mạnh của hãng Siemens. PLC S7-200 gồm các phần chính sau: - Khối CPU. - Khối nhớ RAM, ROM, EPROM, EEPROM. - Khối vào – ra. - Khối nguồn. - Khối mở rộng. Hình 1.1: Cấu tạo chung một PLC. Võ Hoàng Trôvi – Nguyễn Sĩ Bích 4 Power Supply Input Interface Central Processing Unit ( CPU) Memory Output Interface Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh CPU 224 có hai loại thông dụng dựa vào ký hiệu trên nắp máy bao gồm:CPU 224 DC / DC / DC, CPU 224 AC / DC / RLY. Loại CPU 224 DC / DC / DC: Cần được cấp nguồn điện một chiều DC 24V, các đầu vào và đầu ra cũng cần được cấp nguồn điện DC 24 V. Sơ đồ đấu dây : Hình 1.2 : Sơ đồ đấu dây Loại CPU 224 DC / DC / DC Loại CPU 224 AC / DC / RLY: Cần được cấp nguồn điện xoay chiều một pha 220ACV, các đầu vào cần được cấp nguồn điện DC 24 V và các đầu ra là các relay. Sơ đồ đấu dây : Hình 1.3 : Sơ đồ đấu dây Loại CPU 224 AC / DC / RLY 1.1.2 Bộ nhớ PLC S7-200: Võ Hoàng Trôvi – Nguyễn Sĩ Bích 5 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh Bộ nhớ của PLC thường có 3 vùng nhớ chính: 1.1.2.1 Vùng nhớ chứa chương trình ứng dụng: - OB1( Organisation block): vùng nhớ chứa chương trình chính, PLC luôn quét các lệnh trong vùng nhớ này. - Subroutine ( chương trình con): vùng nhớ chứa chương trình con, chương trình con được thực hiện khi được gọi bởi chương trình chính. - Interrup ( chương trình ngắt): vùng nhớ chứa chương trình ngắt, chương trình này sẽ thực hiện khi có một ngắt xảy ra, như: ngắt Timer, ngắt của HSC… 1.1.2.2 Vùng nhớ chứa tham số: Thông thường có 5 vùng như sau: - Vùng nhớ I ( Process image input): vùng dữ liệu các cổng vào số, khi thực hiện chương trình, PLC sẽ đọc giá trị logic tất cả các cổng đầu vào rồi cất chúng trong vùng nhớ I. Chương trình sẽ đọc giá trị logic các cổng vào thông qua vùng nhớ I. - Vùng nhớ Q ( Process Image Output): vùng nhớ đệm cho các cổng ra số. Khi kết thúc thực hiện chương trình, PLC sẽ chuyển giá trị logic của bộ đệm Q tới các cổng ra số. - Vùng nhớ M: chương trình sử dụng các tham số này cho việc lưu giữ các biến cần thiết. Vùng nhớ này có thể truy cập theo bit (M), byte (MB), từ (MW) hay từ kép (MD). - Vùng nhớ T (Timer): dùng để lưu trữ giá trị đặt trước, giá trị hiện tại cũng như giá trị đầu ra của Timer. - Vùng nhớ C ( Counter): dùng để lưu trữ giá trị đặt trước, giá trị hiện tại cũng như giá trị đầu ra của Counter. 1.1.2.3 Vùng chứa các khối dữ liệu: - Data Block: vùng chứa dữ liệu được chia thành khối. Kích thước do người sử dụng quy định. Vùng nhớ này có thể truy cập theo từng bit (DBX), byte(DBB), từ (DBW) hoặc từ kép (DBD). - Local Data Block: vùng dữ liệu địa phương, các khối chương trình chính, chương trình con, chương trình ngắt sử dụng và tổ chức cho các biến nháp tạm thời. Nội dung của khối dữ liệu trong vùng nhớ này khi kết thúc chương trình tương ứng Võ Hoàng Trôvi – Nguyễn Sĩ Bích 6 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh trong chương trình chính, chương trình con hay chương trình ngắt. Vùng nhớ này có thể truy cập theo từng bit (L), byte(LB), từ (LW) hoặc từ kép (LD). 1.1.3 Bộ điều rộng xung (PWM, PTO): CPU S7-200 có 2 ngõ ra xung tốc độ cao Q0.0, Q0.1, dùng cho việc điều rộng xung tốc độ cao trong các ứng dụng điều khiển thiết bị bên ngoài như điều khiển tốc độ động cơ, điều khiển nhiệt độ v.v Có 2 cách điều rộng xung: điều rộng xung 50% (PTO) và điều rộng xung theo tỉ lệ (PWM). Ở chế độ PWM, chu kì xung có thể ở mili giây hoặc micro giây. Chu kì ở tầm 50ms đến 65,535ms hoặc ở 2μs đến 65,535 μs. Ở chế độ PTO, chu kì ở tầm 50ms đến 65,535ms hoặc ở 2μs đến 65,535 μs và xung đếm ở tầm từ 1 đến 4,294,967,295 xung. Để tạo chương trình điều rộng xung thì có thể được thực hiện thông qua việc định dạng Wizard. 1.1.3.1 Điều rộng xung 50% (PTO): Để thực hiện việc phát xung tốc độ cao ở chế độ PTO trước hết ta phải thực hiện các bước định dạng sau:  Reset ngõ xung tốc độ cao ở chu kì đầu của chương trình.  Chọn loại ngõ ra phát xung tốc độ cao Q0.0 hay Q0.1.  Định dạng thời gian cơ sở ( Time base) dựa trên bảng sau: Tham chiếu các byte điều khiển cho chế độ PTO. Result of executing the PLS instruction Control Register (Hex Value) Enable Select Mode PTO Segment Operation Time Base Pulse Count Cycle Time 16#81 Yes PTO Single 1 µs/cycle Load 16#84 Yes PTO Single 1 µs/cycle Load 16#85 Yes PTO Single 1 µs/cycle Load Load 16#89 Yes PTO Single 1 ms/cycle Load 16#8C Yes PTO Single 1 ms/cycle Load 16#8D Yes PTO Single 1 ms/cycle Load Load 16#A0 Yes PTO Multiple 1 µs/cycle 16#A8 Yes PTO Multiple 1 ms/cycle Các Byte cho việc định dạng : SMB67 ( ngõ ra Q0.0) SMB77 ( ngõ ra Q0.1) Ngoài ra: Q0.0 Q0.1 Võ Hoàng Trôvi – Nguyễn Sĩ Bích 7 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh SMW68 SMW78 : Xác định thời gian 1 chu kì. SMD72 SMD82 : Xác định giá trị đếm xung PTO. 1.1.3.2 Điều rộng xung theo tỉ lệ (PWM): Để thực hiện việc phát xung tốc độ cao ở chế độ PWM trước hết ta phải thực hiện các bước định dạng sau:  Reset ngõ xung tốc độ cao ở chu kì đầu của chương trình.  Chọn loại ngõ ra phát xung tốc độ cao Q0.0 hay Q0.1.  Định dạng thời gian cơ sở ( Time base) dựa trên bảng sau: Tham chiếu các byte điều khiển cho chế độ PWM. Các Byte cho việc định dạng: SMB67 ( ngõ ra Q0.0) SMB77 ( ngõ ra Q0.1) Ngoài ra: Q0.0 Q0.1 SMW68 SMW78 : Xác định thời gian 1 chu kì. SMW70 SMW80 : Xác định độ rộng xung PWM. Ta có thể định dạng ngõ ra xung tốc độ cao thông qua các bước định dạng bằng Wizard: - Vào Wizard chọn PTO/PWM. Võ Hoàng Trôvi – Nguyễn Sĩ Bích Result of executing the PLS instruction Control Register (Hex Value) Enable Select Mode PWM Update Method Time Base Pulse Width Cycle Time 16#D1 Yes PWM Synchronous 1 µs/cycle Load 16#D2 Yes PWM Synchronous 1 µs/cycle Load 16#D3 Yes PWM Synchronous 1 µs/cycle Load Load 16#D9 Yes PWM Synchronous 1 ms/cycle Load 16#DA Yes PWM Synchronous 1 ms/cycle Load 16#DB Yes PWM Synchronous 1 ms/cycle Load Load 8 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh - Chọn ngõ ra xung Q0.0 hay Q0.1. - Chọn loại xung PTO hay PWM. Sau đó chọn tốc độ Min max và tốc độ ban đầu. Võ Hoàng Trôvi – Nguyễn Sĩ Bích 9 Đồ án tốt nghiệp GVHD: ThS. Trần Văn Trinh Chọn thời gian khởi động đạt Max và thời gian Stop về Min. Sau đó chọn Byte bắt đầu: Võ Hoàng Trôvi – Nguyễn Sĩ Bích 10 [...]... q1 + h q2 q2 + h q2 q3 + h q3 q1 + 3 311 31 2 31 3 32 1 32 2 32 3 33 1 + h 332 q q + h 333 q q 3 2 3 3 rong đó: T h h h h h h h h h 31 1 = Tr (U 31 1 I 3U 33 ) 31 2 = Tr (U 31 2 I 3U 33 ) 31 3 = Tr (U 31 3 I 3U 33 ) 32 1 = Tr (U 32 1 I 3U 33 ) 32 2 = Tr (U 32 2 I 3U 33 ) 32 3 = Tr (U 32 3 I 3U 33 ) 33 1 = Tr (U 33 1 I 3U 33 ) 33 2 = Tr (U 33 2 I 3U 33 ) 33 3 = Tr (U 33 3 I 3U 33 ) T T T T T T T T Trong đó: U ijk U 111  j −1 Q k... 31 1 I 3U 32 ) 212 = Tr (U 212 I 2U 22) + Tr (U 31 2 I 3U 32 ) 2 13 = Tr (U 31 3 I 3U 32 ) 221 = Tr (U 221 I 2U 22) + Tr (U 32 1 I 3U 32 222 = Tr (U 222 I 2U 22) + Tr (U 32 2 I 3U 32 2 23 = Tr (U 32 3 I 3U 32 ) 231 = Tr (U 33 1 I 3U 32 ) 232 = Tr (U 33 2 I 3U 32 ) 233 = Tr (U 33 3 I 3U 32 ) T T T T T T T T T T T Tính h3 h = h q1 q1 + h q1 q2 + h q1 q3 + h q2 q1 + h q2 q2 + h q2 q3 + h q3 q1 + 3 311 31 2 31 3... 222 I 2U 21) + Tr (U 32 2 I 3U 31 1 23 = Tr (U 32 3 I 3U 31 ) 131 = Tr (U 33 1 I 3U 31 ) 132 = Tr (U 33 2 I 3U 31 ) 133 = Tr (U 33 3 I 3U 31 ) T T T T T T T T T Tính h2 Võ Hoàng Trôvi – Nguyễn Sĩ Bích 33 Đồ án tốt nghiệp GVHD: ThS Trần Văn Trinh h2 = h211 q1 q1 + h212 q1 q2 + h2 13 q1 q3 + h221 q2 q1 + h222 q2 q2 + h2 23 q2 q3 + h 231 q3 q1 + + h 232 q q + h 233 q q 3 2 3 3 trong đó: h h h h... Tr (U 32 I 3U 32 ) 23 = Tr (U 33 I 3U 32 ) 31 = Tr (U 31 I 3U 33 ) 32 = Tr (U 32 I 3U 33 ) 33 = Tr (U 33 I 3U 33 ) D D D D D D D D D T T T T T T T T T T T Trong đó: U U U ij i  j −1 ;j≤i  = T 0 Q j T j −1  0; j > i  11 = Q T 0 ; U 21 = Q T 0 ; U 31 = Q T 0 1 1 1 22 = T 0Q 1 1 2 2 2 T ;U 1 = T 0Q 3 1 32 3 2 T ;U 1 = T 0Q 2 33 3 T 3 2 * Tính thông số h 3 3 hi = ∑∑ hikm q h ikm k =1 m =1 3 k qm ∑ Tr... U 31 1 = Q1 T 0 2 Võ Hoàng Trôvi – Nguyễn Sĩ Bích 3 34 t Đồ án tốt nghiệp U U U U GVHD: ThS Trần Văn Trinh = U 221 = Q T 0 Q 1 212 2 T ;U =U = Q T Q T =U = Q T Q T ; U =T Q Q T 1 2 1 2 32 1 1 0 33 1 1 0 2 33 3 0 2 2 T 2 1 3 2 31 3 1 1 1 31 2 = T 0Q 222 3 3 2 = U 33 2 = T 0 Q 1 32 3 T QT 2 2 1 3 3 2 3 3 2 2 Tính Ci C − 3 i = −∑ mi g U ji r j j j =i − − − C1 = − m1 g U 11 r11 − m2 g U 21 r22 − m3 g U 31 r 33. .. ) T j jkm ji ) Tính h1 h1 = h111 q1 q1 + h112 q1 q2 + h1 13 q1 q3 + h121 q2 q1 + h122 q2 q2 + h1 23 q2 q3 + h 131 q3 q1 + + h 132 q q + h 133 q q 3 2 3 3 trong đó: h h h h h h h h h T T T T T 111 = Tr (U 111 I 1U 11) + Tr (U 211 I 2U 21) + Tr (U 31 1 I 3U 31 ) 112 = Tr (U 212 I 2U 21) + Tr (U 31 2 I 3U 31 ) 1 13 = Tr (U 31 3 I 3U 31 ) 121 = Tr (U 221 I 2U 21) + Tr (U 32 1 I 3U 31 122 = Tr (U 222... Sθ 1Cθ 23 Sθ 1Sθ 23 Cθ 1 − l 3Sθ 1Cθ 23 − l 2 Sθ 1Cθ 2 − Cθ 1Cθ 23 − Cθ 1Sθ 23 Sθ 1 l 3Cθ 1Cθ 23 + l 2Cθ 1Cθ 2  ; U 31 =  0   0 0 0   0 0 0 0   Võ Hoàng Trôvi – Nguyễn Sĩ Bích 35 Đồ án tốt nghiệp GVHD: ThS Trần Văn Trinh − Cθ 1Sθ 23 − Cθ 1Cθ 23  − Sθ 1Sθ 23 − Sθ 1Cθ 23 = U 32  Cθ 23 − Sθ 23  0 0  0 − l 3Cθ 1Sθ 23 − l 2Cθ 1Sθ 2 0 − l 3Sθ 1Sθ 23 − l 2 Sθ 1Sθ 2  ;  0 l 3Cθ 23 + l 2Cθ... 1.2 .3. 1 Phân loại theo số bậc tự do: Robot thường phân loại theo số bậc tự do Trong không gian làm việc ba chiều thì lý tưởng Robot phải có 6 bậc tự do Robot có hơn 6 bậc tự do thì dư mà ít hơn 6 Võ Hoàng Trôvi – Nguyễn Sĩ Bích 20 Đồ án tốt nghiệp GVHD: ThS Trần Văn Trinh bậc tự do thì thiếu Robot có số bậc tự do lớn hơn 6 đôi khi dùng để di chuyển Trong một số trường hợp thì 4 bậc tự do cũng đủ yêu cầu làm... bàn tay phải: 1 .3. 2 Động học và động lực học thuận: Hình 1.22:Hệ trục tọa độ gắn cho tay máy 3 bậc tự do 1 .3. 2.1 Bảng thông số D-H: Khớp i 1 2 3 αi 90 0 0 ai 0 l2 l3 Di 11 0 0 θi θ1 θ2 3 i = 1,2 ,3; n =3 Võ Hoàng Trôvi – Nguyễn Sĩ Bích 30 Đồ án tốt nghiệp GVHD: ThS Trần Văn Trinh 1 .3. 2.2 Các ma trận biến đổi đồng nhất: Cθ 1  Sθ 1 1 T0 =   0   0 0 Sθ 1 0 − Cθ 1 1 0 0 0 Cθ 3 − Sθ 3  Sθ 3 Cθ 3 3... 1  0 l 3Cθ 3 0 l 3Sθ 3  ; 1 0   0 1  T02 = T01 × T12 T 03 = T02 × T 23 ; 3 2 3 1 1 2 T = T ×T Cθ 1Cθ 2 − Cθ 1Sθ 2 Sθ 1 l 2Cθ 1Cθ 2  Sθ 1Cθ 2 − Sθ 1Cθ 2 − Cθ 1 l 2 Sθ 1Cθ 2  2 ; T0 =   Sθ 2 Cθ 2 0 l 2 Sθ 2 + l1    0 0 1  0  Cθ 23 − Sθ 23  Sθ 23 Cθ 23 3 T1 =   0 0  0  0 0 l 3Cθ 23 + l 2Cθ 2 0 l 3Sθ 23 + l 2Sθ 2    1 0  0 1  Cθ 1Cθ 23 − Cθ 1Sθ 23 Sθ 1 l 3Cθ 1Cθ 23 + l 2Cθ . tử tự động, để bổ sung những kiến thức đã học cũng như nghiên cứu những vấn đề mới trong lĩnh vực điều khiển nên chúng em quyết định chọn đề tài Điều khiển tay máy 3 bậc tự do dùng thị giác máy. máy tính . Thông qua luận văn chúng em sẽ khảo sát, nghiên cứu và điều khiển tay máy 3 bậc tự do để gắp vật cố định trên mặt phẳng nhờ sự trợ giúp bởi Camera xác định tọa độ vật. Để điều khiển tay. Description SM36.0 SM46.0 SM56.0 SM 136 .0 SM146.0 SM156.0 Not used SM36.1 SM46.1 SM56.1 SM 136 .1 SM146.0 SM156.1 Not used SM36.2 SM46.2 SM56.2 SM 136 .2 SM146.0 SM156.2 Not used SM36 .3 SM46 .3 SM56 .3 SM 136 .3