ĐẠI HỌC QUỐC GIA HÀ NỘI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ o0o Đặng Anh Việt ĐIỀU KHIỂN PHÂN TÁN ROBOT CÔNG NGHIỆP SEPHENT THEO QUỸ ĐẠO ĐỊNH TRƯỚC LUẬN VĂN THẠC SĨ Hà Nội - 2006 MỤC LỤC Mục lục 1 Danh mục các hình vẽ, bảng 4 Mở đầu 6 CHƢƠNG 1: Tổng quan 7 1.1. Đặt vấn đề 7 1.2. Một số cấu trúc hệ thống điều khiển 8 1.2.1. Hệ thống điều khiển theo cấu trúc tập trung 8 1.2.2. Cấu trúc điều khiển phân tán 9 1.3. Nội dung của luận văn 12 CHƢƠNG 2: Đối tƣợng ứng dụng - ROBOT SHEPHENT 14 2.1. Giới thiệu robot shephent 14 2.2. Động học vị trí của robot SHEPHENT 15 2.2.1. Cấu hình Robot Scara – SHEPHENT: 15 2.2.2. Động học thuận Robot SHEPHENT . 15 2.2.3. Động học ngƣợc Robot SHEPHENT . 19 2.3. Xây dựng hệ điều khiển phân tán cho robot shephent 22 2.3.1. Cấu trúc hệ thống điều khiển phân tán 22 2.3.2. Chiến lƣợc điều khiển phân tán 23 2.3.3. Phần mềm điều khiển chạy trên máy tính 24 2.3.4. Thuật toán điều khiển tại các bộ điều khiển từng khớp 24 2.4. Các vấn đề đặt ra với hệ thống điều khiển phân tán robot shephent 26 2.4.1. Tính quỹ đạo rời rạc theo thời gian định trƣớc 26 2.4.2. Truyền dữ liệu, giám sát bộ điều khiển thực hiện quỹ đạo và đồng bộ chuyển động của các khớp 26 2.4.3. Xây dựng các bộ điều khiển động cơ và các bộ thu thập số liệu, điều khiển các thiết bị chấp hành khác 27 CHƢƠNG 3: Module tính quỹ đạo 28 2 3.1. Một số phƣơng pháp tạo quỹ đạo 28 3.1.1. Tạo quỹ đạo cho các chuyển động PTP 29 3.1.2. Tạo quỹ đạo cho các chuyển động CP 35 3.1.3. Một thuật toán nội suy quỹ đạo với nhiều điểm tựa 43 3.2. Xây dựng module nội suy quỹ đạo, vận tốc cho robot shephent 45 3.2.1. Triển khai các module nội suy quỹ đạo trên C ++ 45 3.2.2. Module nội suy bậc 3 – Cnoisuyx3 45 3.2.3. Module nội suy quỹ đạo n điểm - CnoisuyQDx3 47 3.2.4. Một số ví dụ ứng dụng thuật giải 47 3.3. Yêu cầu cho hệ thống truyền dữ liệu quỹ đạo đến các bộ điều khiển phân tán 49 CHƢƠNG 4: Xây dựng cơ sở truyền thông trong hệ điều khiển phân tán 50 4.1. Mạng truyền thông công nghiệp CAN 51 4.1.1. Kiến trúc giao thức 51 4.1.2. Các cấu trúc mạng và kỹ thuật truyền dẫn 52 4.1.3. Cơ chế giao tiếp 53 4.2. Vi xử lý Atmel AT89c52cc02 56 4.2.1. Các tính năng chính 56 4.2.2. Bộ điều khiển truyền thông CAN (CAN Controller) của AT89C51CC02 58 4.3. Vi điều khiển tích hợp bộ xử lý tín hiệu số Microchip dsPic30F4011 60 4.3.1. Các tính năng chính của dsPIP30F401: 60 4.3.2. Các module chuyên dụng cho điều khiển động cơ của vi điều khiển dsPIP30F4011 62 4.3.3. Bộ điều khiển truyền thông CAN (CAN Controller) của dsPic30F4011 64 4.4. Xây dựng nền tảng truyền thông tin cho hệ thống 66 4.4.1. Triển khai lớp vật lý 66 4.4.2. Triển khai lớp ứng dụng 66 4.4.3. Cơ chế giao tiếp 67 3 4.4.4. Phân loại thiết bị và dữ liệu trong VCCAN 67 4.4.5. Mô hình đối tƣợng. 68 4.4.6. Mô hình địa chỉ. 69 4.4.7. Cấu trúc bức điện. 69 CHƢƠNG 5: Triển khai phần cứng và các kết quả đã đạt đƣợc 70 5.1. Bộ chuyển đổi USB-CAN 70 5.1.1. Sơ đồ nguyên lý 70 5.1.2. Phƣơng thức hoạt động 71 5.2. Bộ điều khiển động cơ servo một chiều 71 5.2.1. Sơ đồ nguyên lý 71 5.2.2. Phƣơng thức hoạt động 73 5.2.3. Thuật toán điều khiển động cơ 74 5.2.4. Thủ tục homing 76 5.3. Thiết kế, chế tạo robot SHEPHENT, chạy thử nghiệm 77 5.3.1. Thiết kế, chế tạo phần cơ khí robot SHEPHENT 77 5.3.2. Kết quả thiết kế, chế tạo mạch điện tử 78 5.3.3. Kết quả chạy thử nghiệm 79 5.3.4. Các việc đang đƣợc tiến hành để hoàn thiện hệ thống 79 Kết luận 81 Tài liệu tham khảo 82 4 DANH MỤC CÁC HÌNH VẼ, BẢNG Hình 1.1: Cấu trúc một hệ điều khiển truyền động truyền thống 8 Hình 1.2: Một hệ điều khiển truyền động truyền thống 9 Hình 1.3: Cấu trúc một hệ điều khiển truyền động phân tán 10 Hình 1.4: Một hệ điều khiển truyền động phân tán 11 Hình 2.1: Robot SHEPHENT 14 Hình 2.2: Cấu hình robot SHEPHENT 15 Hình 2.3: Giải bài toán động học ngƣợc robot Shephent bằng hình học 19 Hình 2.4: Cấu trúc hệ thống điều khiển phân tán robot Shephent 22 Hình 2.5: Cấu trúc phần mềm điều khiển 24 Hình 2.6: Thuật toán điều khiển 25 Hình 3.1: Chuyển động PTP giữa hai khớp nối 30 Hình 3.2: Đƣờng đi với dạng tốc độ hình thang 31 Hình 3.3: Dạng đƣờng hình sin 33 Hình 3.4: Nội suy theo đƣờng thẳng để xác định vị trí của TCP 36 Hình 3.5: Thuật toán nội suy cho các chuyển động theo đƣờng thẳng 38 Hình 3.6: Nội suy theo đƣờng tròn để xác định vị trí của TCP 40 Hình 3.7: Mềm hoá các đoạn gấp khúc 40 Hình 3.8: Các đa thức Bernstein với n=2 (bậc 2) và n=3 (bậc 3) 41 Hình 3.9: Ví dụ về các đƣờng cong Bezier 42 Hình 3.10: Chia liên tục đa giác 42 Hình 3.11: Ví dụ ứng dụng thuật toán nội suy 47 Hình 3.12: Ví dụ ứng dụng thuật toán nội suy qua 3 điểm 48 Hình 3.13: Ví dụ ứng dụng thuật toán nội suy qua 4 điểm 48 Hình 3.14: Ví dụ ứng dụng thuật toán nội suy qua 6 điểm 48 Hình 4.1: Phạm vi định nghĩa của CAN trong mô hình OSi 52 Hình 4.2: Sơ đồ khối của vi xử lý AT89C51CC02 57 5 Hình 4.3: Sơ đồ khối bộ điều khiển truyền thông CAN của vi xử lý AT89C51CC02 58 Hình 4.4: Sơ đồ các ngắt của bộ điều khiển CAN trong vi xử lý AT89C51CC02 60 Hình 5.1: Mạch nguyên lý bộ chuyển đổi USB-CAN 70 Hình 5.2: Mạch nguyên lý phần vi điều khiển bộ điều động cơ một chiều 71 Hình 5.3: Mạch nguyên lý phần công suất bộ điều khiển động cơ một chiều 72 Hình 5.4: Mạch nguyên lý phần vi điều khiển bộ điều khiển động cơ một chiều không chổi quét 72 Hình 5.5: Mạch nguyên lý phần công suất bộ điều khiển động cơ một chiều không chổi quét 73 Hình 5.6: Thuật toán điều khiển vị trí động cơ 75 Hình 5.7: Thuật toán homing 76 Hình 5.8: Robot SHEPHENT 77 Hình 5.9: Đi dây trong thân robot SHEPHENT 77 Hình 5.10: Các cơ cấu truyền động trong robot SHEPHENT 78 Hình 5.11: Bộ điều khiển động cơ một chiều 78 Hình 5.12: Các bộ điều khiển động cơ đƣợc kết nối qua mạng VCCAN 79 Bảng 2-1: Tham số của Robot SHEPHENT 16 6 MỞ ĐẦU Theo định hƣớng phát triển Tự động hoá tại Việt Nam, việc phát triển các robot công nghiệp đang đƣợc nhiều đơn vị chú trọng. Những robot này làm việc thay con ngƣời trong rất nhiều lĩnh vực nhƣ: đóng gói sản phẩm, hàn, phun sơn, lắp ráp Chúng đƣợc ứng dụng rộng rãi trong công nghiệp để thay thế lao động của con ngƣời, đặc biệt là làm thay con ngƣời những công việc nặng nhọc, độc hại, nguy hiểm. Các robot đƣợc điều khiển bằng máy tính hoặc các bộ vi xử lí, chúng có thể dễ dàng lập trình để thực hiện các tác vụ khác nhau. Bên cạnh đó các bộ phận đƣợc chế tạo dƣới dạng môdun hoá làm cho việc thay đổi, mở rộng tính năng đƣợc dễ dàng và linh hoạt. Việc xây dựng các hệ thống điều khiển phân tán cho robot công nghiệp nói riêng và các hệ truyền động nói chung đang là một hƣớng đi khả thi và hứa hẹn mang lại hiệu quả kinh tế cao. Các hệ thống này bên cạnh giá thành rẻ còn có khả năng mở rộng và tính linh hoạt rất cao, đặc biệt là cho việc nâng cấp các máy công cụ thành các máy CNC và các hệ thống truyền động không yêu cầu độ chính xác quá cao. Trong điều kiện Việt Nam hiện nay, khi xây dựng các hệ thống điều khiển chuyển động phân tán, nền tảng truyền thông trong hệ thống đều dựa vào các thiết bị đắt tiền của nƣớc ngoài. Luận văn hƣớng tới việc xây dựng một hệ thống điều khiển phân tán cho các hệ truyền động với một chuẩn truyền thông sử dụng các linh kiện rẻ tiền, dựa trên các chuẩn mở. Với phạm vi hẹp của các hệ điều khiển chuyển động, ngoài tính mở và khả thi, chuẩn truyền thông này còn yêu cầu một sự kết hợp hợp lý giữa các phần cơ khí thể hiện bằng các biến trong các phƣơng trình cơ, các thuật toán điều khiển và khả năng truyền thông bởi các thiết bị sử dụng trong hệ thống. Tác giả xin chân thành cảm ơn TS. Nguyễn Hồng Quang, GS. TSKH. Đỗ Sanh, cùng toàn thể các thầy cô giáo thuộc trung tâm hợp tác đào tạo Viện Cơ học đã hƣớng dẫn, giúp đỡ tác giả hoàn thành đồ án này. Hà Nội, 8/2005 Đặng Anh Việt 7 CH¦¥NG 1: TỔNG QUAN 1.1. ĐẶT VẤN ĐỀ Hiện nay, ở Việt Nam hầu hết chúng ta phải nhập các dây chuyền máy móc từ nƣớc ngoài trong đó không thể thiếu phần truyền động. Các hệ thống truyền động này thƣờng sử dụng các bộ điều khiển truyền động và phần công suất có khả năng kết nối truyền thông mạng với nhau nhƣng chúng có giá rất cao. Hệ thống máy móc này thƣờng đƣợc các chuyên gia nƣớc ngoài sau khi lắp đặt và chạy thử tại Việt Nam thì sẽ tiến hành đào tạo và chuyển giao công nghệ cho phía Việt Nam. Mặc dù vậy khi hệ thống gặp sự cố thì do nhiều nguyên nhân mà chúng ta cũng không thể khắc phục đƣợc khi đó lại phải cần đến các chuyên gia nƣớc ngoài. Việc đó có ảnh hƣởng nhiều mặt, trong đó ảnh hƣởng đến nền kinh tế vẫn là lớn nhất do chúng ta phải nhập dây chuyền máy móc từ nƣớc ngoài cộng với chi phí chuyên gia cực cao. Bên cạnh đó có một bài toán thực tế là nhu cầu về các máy CNC hoặc tay máy công nghiệp tại các phân xƣởng sản xuất ngày càng cao. Tuy nhiên vốn đầu tƣ cho các thiết bị mới rất lớn cũng nhƣ các hệ thống cơ khí chính xác Việt Nam chƣa sản xuất đƣợc nên một giải pháp hợp lý là nâng cấp các hệ thống máy gia công có sẵn thành máy CNC hoặc mua các hệ thống CNC cũ, hỏng của nƣớc ngoài về và sửa chữa, nâng cấp. Do đó, việc tự nghiên cứu sản xuất toàn bộ hệ thống truyền động, hay ít nhất là tự chế tạo đƣợc các bộ điều khiển các hệ thống máy có sẵn hứa hẹn mang lại hiệu quả kinh tế cao, có thể tiết kiệm cho ngân sách Nhà nƣớc một khoản tài chính rất lớn. Xuất phát từ mong muốn xây một hệ thống điều khiển cho các hệ truyền động, cụ thể là cho các máy CNC hoặc các tay máy công nghiệp, tác giả hƣớng tới việc nghiên cứu một hệ thống điều khiển phân tán và giải quyết một số bài toán đặt ra cho hệ thống điều khiển phân tán, kết quả đƣợc ứng dụng trên một đối tƣợng cụ thể là robot SHEPHENT. Hệ thống điều khiển cần xây dựng có thể không yêu cầu độ chính xác cao hoặc thời gian đáp ứng nhanh, nhƣng phải rẻ tiền, linh hoạt và quan trọng nhất là có thể đƣợc thiết kế, chế tạo tại Việt Nam. Ngoài ra, hệ thống này cần đƣợc xây dựng dựa theo các chuẩn quốc tế, tiến tới hợp chuẩn để có thể kết nối với các thiết bị cao cấp hơn của nƣớc ngoài. 8 1.2. MỘT SỐ CẤU TRÚC HỆ THỐNG ĐIỀU KHIỂN 1.2.1. Hệ thống điều khiển theo cấu trúc tập trung Các hệ thống truyền động truyền thống dựa trên cấu trúc tập trung. Trong cấu trúc này, một bộ điều khiển trung tâm sẽ làm toàn bộ nhiệm vụ thu thập dữ liệu phản hồi trong quá trình hoạt động, trên cơ sở đó đƣa ra tín hiệu điều khiển cho các bộ điều khiển động cơ. Các bộ điều khiển động cơ này chủ yếu là các bộ điều khiển tốc độ hoặc thuần tuý là mạch khuếch đại công suất. Các tín hiệu điều khiển là các tín hiệu tƣơng tự (analog). Cấu trúc hệ điều khiển tập trung thể hiện trong hình vẽ 1.1 Hình 1.1: Cấu trúc một hệ điều khiển truyền động truyền thống Cấu trúc tập trung này yêu cầu một bộ vi xử lý trung tâm cực mạnh nên giá thành cao. Bên cạnh đó, khối lƣợng dây dẫn tín hiệu và chống nhiễu cho các dây tín hiệu cũng dẫn tới nhiều khó khăn khi thiết kế và thi công hệ thống. Một hệ thống điều khiển truyền động điển hình, đang đƣợc sử dụng nhiều tại Việt Nam của hãng TrioMotion thể hiện trong hình 1.2 Bộ điều khiển trung tâm Bộ điều khiển động cơ 1 (khớp 1) Bộ điều khiển động cơ 2 (khớp 2) Bộ điều khiển động cơ 3 (khớp 3) Các đầu vào ra số/tƣơng tự động cơ 1 Phản hồi vị trí động cơ 3 Phản hồi vị trí động cơ 2 Phản hồi vị trí 9 Hình 1.2: Một hệ điều khiển truyền động truyền thống Mặc dù không thể phủ nhận tốc độ đáp ứng và độ chính xác cao của các hệ thống này, nhƣng giá thành và kết nối là những nhƣợc điểm lớn, không mang lại hiệu quả cao trong điều kiện Việt Nam. 1.2.2. Cấu trúc điều khiển phân tán Cùng với sự phát triển của lý thuyết điều khiển phân tán và sự phát triển nhanh chóng của các mạng truyền thông công nghiệp, các hãng sản xuất thiết bị công nghiệp cũng đã phát triển các hệ truyền động theo cấu trúc phân tán. Trong cấu trúc phân tán, các bộ điều khiển động cơ đƣợc tích hợp cả chức năng điều khiển vị trí và đƣợc nối mạng với nhau thông qua một mạng truyền thông. Hệ thống có thể có hoặc không cần một bộ điều khiển trung tâm. Tuy nhiên, bộ điều khiển trung tâm này (nếu có) thì chỉ giữ chức năng chính là đồng bộ các chuyển động và truyền dữ liệu về quỹ đạo yêu cầu cho các bộ điều khiển. Cấu trúc điều khiển phân tán hệ truyền động đƣợc thể hiện trong hình 1.3 [...]... THỐNG ĐIỀU KHIỂN PHÂN TÁN ROBOT SHEPHENT Với chiến lƣợc điều khiển phân tán đã đặt ra, có 4 vấn đề lớn cần phải giải quyết:  Nội suy quỹ đạo dựa trên 2 điểm đầu cuối với quỹ đạo điểm-điểm hoặc dựa trên các điểm tựa với quỹ đạo đƣờng liên tục để tính các điểm quỹ đạo rời rạc theo thời gian định trƣớc  Truyền dữ liệu tính toán đƣợc này đến từng bộ điều khiển, giám sát việc bộ điều khiển thực hiện quỹ đạo, ... cơ 2 (khớp 2) Bộ điều khiển động cơ 3 (khớp 3) Bộ điều khiển động cơ 4 (khớp 4) Các đầu vào Các đầu vào Teach ra số/tƣơng tự ra số/tƣơng tự pendant Hình 2.4: Cấu trúc hệ thống điều khiển phân tán robot Shephent Hệ thống điều khiển phân tán robot Shephent đƣợc xây dựng từ những bộ điều khiển riêng cho từng động cơ Các bộ điều khiển động cơ này, cùng với các bộ đo lƣờng và điều khiển khác (các đầu vào...10 Mạng truyền thông Bộ điều khiển Bộ điều khiển Bộ điều khiển Các vào động cơ 1 (khớp 1) động cơ 2 (khớp 2) động cơ 3 (khớp 3) số/tƣơng tự động cơ 1 động cơ 2 động cơ 3 Phản vị trí Phản vị trí Phản vị trí hồi hồi hồi đầu ra Bộ điều khiển trung tâm Hình 1.3: Cấu trúc một hệ điều khiển truyền động phân tán Thị phần về thiết bị truyền động, đặc biệt là các hệ truyền động phân tán tại Việt nam đang thuộc... mỗi mức độ chuyển động Vấn đề cơ bản trong điều khiển quỹ đạo robot là di chuyển robot từ vị trí đầu, xác định bởi tool frame Tinitial đến vị trí cuối xác định bởi tool frame Tfinal Với một tay máy robot thì vị trí của điểm đầu tay máy và hƣớng tác động là các điểm cần xác định (Tool Center Point - TCP) Các vấn đề đặt ra:  Xác định các quỹ đạo dựa trên các điểm tựa với điều khiển quỹ đạo theo đƣờng... đồng bộ chuyển động của các khớp  Xây dựng bộ điều khiển động cơ cho từng khớp  Xây dựng bộ điều khiển các đầu vào/ra để thu thập dữ liệu của các cảm biến cũng nhƣ điều khiển các cơ cấu chấp hành khác nhƣ xi lanh khí nén 2.4.1 Tính quỹ đạo rời rạc theo thời gian định trƣớc Các bộ điều khiển cho các khớp sẽ hoạt động theo nguyên lý điều khiển động cơ đến các vị trí sau các khoảng thời gian nhất định. .. kết quả này xƣống các module điều khiển phân tán thông qua mạng Thuật toán tính toán quỹ đạo, vận tốc không chỉ đảm bảo điều khiển robot di chuyển theo một quỹ đạo liên tục mà còn phải thỏa mãn các giới hạn của các bộ vi xử lý và khả năng truyền thông trên mạng Nhằm hƣớng tới một hệ thống điều khiển chuyển động phân tán, không chỉ phát huy ƣu điểm của các hệ thống phân tán nhƣ giá thành rẻ và khả năng... robot cụ thể SHEPHENT Luận văn đƣợc trình bày thành 5 chƣơng:  Chƣơng 1 - Tổng quan: Phân tích các hệ thống điều khiển robot và hƣớng nghiên cứu cho đề tài  Chƣơng 2 - Đối tƣợng ứng dụng - Robot SHEPHENT: Nghiên cứu một đối tƣợng cụ thể là robot SHEPHENT để phục vụ việc nghiên cứu và kiểm chứng hệ thống điều khiển phân tán cần xây dựng  Mô hình hoá robot SHEPHENT 13  Cấu trúc điều khiển phân tán. .. trình động học ngƣợc nói chung có thể áp dụng trong hầu hết các trƣờng hợp Giải hệ phƣơng trình động học ngƣợc của robot ta tính đƣợc giá trị các khớp quay và khớp tịnh tiến 22 2.3 XÂY DỰNG HỆ ĐIỀU KHIỂN PHÂN TÁN CHO ROBOT SHEPHENT 2.3.1 Cấu trúc hệ thống điều khiển phân tán Kết nối USB-CAN USB PC converter Mạng truyền thông VCCAN Bộ điều khiển động cơ 1 (khớp 1) Bộ điều khiển động cơ 2 (khớp 2) Bộ điều. .. là các điểm mà quỹ đạo của robot bắt buộc phải qua điểm đó tại thời điểm xác định hoặc vận tốc xác định Quỹ đạo của robot sẽ đƣợc nội suy từ các điểm tựa này Một số khả năng xảy ra:  Chuyển động theo đƣờng thẳng (TCP theo một đƣờng thẳng)  Chuyển động tròn (TCP theo một cung tròn)  Chuyển động quanh trục  Chuyển động học ngƣợc cần đƣợc tính toán lúc hoạt động 3.1.2.1 Nội suy theo đường thẳng Cho... chuyển ti cho mỗi khớp nối có thể khác nhau Nói chung mỗi trục có thời gian dịch chuyển khác nhau nên không thoả mãn yêu cầu truyền động chính xác 35 3.1.2 Tạo quỹ đạo cho các chuyển động CP Robot cần đƣợc điều khiển chuyển động giữa điểm đầu và điểm cuối theo một quỹ đạo định trƣớc Quỹ đạo này đƣợc cho bởi các hàm của các toạ độ của TCP hoặc đƣợc cho bởi các điểm tựa Các điểm tựa là các điểm mà quỹ . 2.3. Xây dựng hệ iều khiển phân tán cho robot shephent 22 2.3.1. Cấu trúc hệ thống iều khiển phân tán 22 2.3.2. Chiến lƣợc iều khiển phân tán 23 2.3.3. Phần mềm iều khiển chạy trên máy. 2.3.4. Thuật toán iều khiển tại các bộ iều khiển từng khớp 24 2.4. Các vấn đề đặt ra với hệ thống iều khiển phân tán robot shephent 26 2.4.1. Tính quỹ đạo rời rạc theo thời gian định trƣớc 26. tính quỹ đạo 28 2 3.1. Một số phƣơng pháp tạo quỹ đạo 28 3.1.1. Tạo quỹ đạo cho các chuyển động PTP 29 3.1.2. Tạo quỹ đạo cho các chuyển động CP 35 3.1.3. Một thuật toán nội suy quỹ đạo