Điều khiển bám quỹ đạo cho đầu công tác tay máy 6 bậc tự do

Kỹ thuật điều khiển & Điện tử T. T. Tiến, P. M. Tân, , “Điều khiển bám quỹ đạo cho đầu công tác tay máy 6 bậc tự do.” 44 ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO ĐẦU CÔNG TÁC TAY MÁY 6 BẬC TỰ DO Trần Tân Tiến*, Phạm Minh Tân, Trần Trung Kiên, Đặng Nam Kiên, Nguyễn Duy Trung Tóm tắt: Tay máy ngày càng được ứng dụng rộng rãi với yêu cầu ngày càng cao về độ chính xác. Trong những bài toán thực tế, không chỉ yêu cầu tay máy di chuyển chính xác về tọa độ vị trí (di chuyển điểm-điểm như bài toán gắp nhả và di chuyển sản phẩm không có vật cản) trong không gian, mà còn yêu cầu đầu công tác của tay máy phải di chuyển chính xác trên cả đường quỹ đạo di chuyển liên tục trong không gian thao tác (như bài toán sơn sản phẩm nhiều chi tiết, hàn theo đường liên tục...). Bài viết giới thiệu một phương pháp điều khiển đầu công tác tay máy 6 bậc tự do bám theo quỹ đạo liên tục cho trước. Từ khóa: Tay máy 6 bậc tự do, Quỹ đạo chuyển động, Điều khiển bám. 1. ĐẶT VẤN ĐỀ Trong [1], nhóm tác giả đã trình bày một phương pháp giải bài toán động học cho tay máy 6 bậc tự do (tay máy Hitachi M6100) phục vụ điều khiển đầu công tác di chuyển chính xác các tọa độ trong không gian. Song trong thực tế có nhiều bài toán đòi hỏi đầu công tác của tay máy không chỉ di chuyển chính xác theo tọa độ trong không gian, mà còn phải chính xác trên cả quỹ đạo di chuyển và có thể phải tránh vật cản theo đường thẳng, đường tròn, elip .... Hình 1. Một số quỹ đạo di chuyển cơ bản. Việc điều khiển bám quỹ đạo đầu công tác của tay máy nhằm đáp ứng yêu cầu trên có thể phân chia thành 2 bài toán quan trọng là: xây dựng quỹ đạo chuyển động cho các trục và điều khiển bám đồng thời đồng bộ chuyển động bám của các trục tay máy. Hiện nay, có nhiều phương pháp hiện đại đã được nghiên cứu để điều khiển bám quỹ đạo cho đầu công tác tay máy nhiều bậc tự do như: điều khiển tối ưu [4], điều khiển trượt mờ [5], điều khiển mờ lai [6]... và tương ứng là bài toán xây dựng quỹ đạo chuyển động cho các trục [4], [5], [7]. Các phương pháp này đều giải quyết tốt bài toán điều khiển bám với quỹ đạo bất kỳ trong không gian làm việc của tay máy. Tuy nhiên, bài viết sau sẽ trình bày một phương pháp giải quyết vấn đề đã nêu một cách đơn giản hơn mà vẫn đảm bảo độ chính xác và các yêu cầu kỹ thuật đồng thời dễ dàng triển khai trong một số ứng dụng thực tế. Trong một số ứng dụng của tay máy 6 bậc tự do như: trong hàn đường, sơn sản phẩm nhiều chi tiết... một chu trình làm việc có thể được ghép nối từ nhiều quỹ đạo thành phần; hay trong nhận dạng và phân loại sản phẩm, chỉ cần xây dựng đường cong chuyển động cho đầu công tác đi qua một số điểm nhất định để tránh vật cản... Trên mỗi quỹ đạo thành phần ấy, có thể xấp xỉ quỹ đạo chuyển động cho mỗi trục dưới dạng một đa thức bậc cao theo thời gian. Do đó, bài toán điều khiển bám quỹ đạo đầu công tác trở thành bài toán điều khiển bám cho mỗi trục tay máy theo phương trình chuyển động tương ứng với biến thời gian cụ thể như sau: Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 45 - Trong từng chu kỳ trích mẫu để tính toán tín hiệu điều khiển, việc tính toán quỹ đạo chuyển động được quy về tính toán trên 6 đa thức bậc cao cho 6 trục đã được xấp xỉ từ trước. - Việc đồng bộ chuyển động cho 6 trục được tự động thực hiện do 6 đa thức bậc cao được tính toán trên cùng một biến thời gian. - Chất lượng của hệ thống còn lại là phụ thuộc vào chất lượng của bộ điều khiển bám PIV cho mỗi trục. Việc thiết kế bộ điều khiển bám quỹ đạo cho đầu công tác của tay máy 6 bậc tự do cho những ứng dụng như đã phân tích sẽ được trình bày ở phần sau. 2. THIẾT KẾ BỘ ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO ĐẦU CÔNG TÁC CỦA TAY MÁY 2.1. Xây dựng bài toán điều khiển bám quỹ đạo Giả sử cần điều khiển đầu công tác của tay máy 6 bậc tự do di chuyển theo quỹ đạo tròn trong không gian thao tác, sơ đồ giải pháp thực hiện như trên hình 2. Hình 2. Sơ đồ giải pháp thực hiện. Kỹ thuật điều khiển & Điện tử T. T. Tiến, P. M. Tân, , “Điều khiển bám quỹ đạo cho đầu công tác tay máy 6 bậc tự do.” 46 Từ sơ đồ giải pháp, nhận thấy để thực hiện được yêu cầu điều khiển đầu công tác di chuyển theo quỹ đạo liên tục trong không gian cần giải quyết được 2 bài toán cơ bản sau: Bài toán 1: Xây dựng quỹ đạo chuyển động cho các trục. - Xây dựng phương trình quỹ đạo chuyển động của đầu công tác theo thời gian. - Rời rạc hóa quỹ đạo chuyển động thành nhiều điểm theo thời gian, thu được quỹ đạo chuyển động rời rạc của đầu công tác. - Giải bài toán động học ngược cho từng điểm rời rạc ấy, chọn nghiệm theo phương pháp tối ưu khoảng cách dịch chuyển ta sẽ thu được quỹ đạo chuyển động rời rạc cho mỗi trục. - Xấp xỉ hàm cho từng trục từ chuỗi những điểm rời rạc tương ứng bằng phương pháp bình phương tối thiểu, thu được quỹ đạo chuyển động liên tục cho mỗi trục là 1 đa thức bậc cao theo theo thời gian, có dạng: θi = fi(t). Tùy theo độ phức tạp của quỹ đạo cũng như yêu cầu về độ chính xác để lựa chọn bậc của đa thức. Trong đó: θi: góc quay trục thứ i (i=1÷6); fi(t): hàm xấp xỉ trục thứ i (i =1÷6) Lời giải cụ thể cho bài toán này được trình bày trong [3]. Bài toán 2: Điều khiển bám và đồng bộ chuyển động các trục. - Xây dựng bộ điều khiển bám cho mỗi trục (PIV), để mỗi trục chuyển động bám theo quỹ đạo chuyển động riêng đã được lập trình ở trên. - Đồng bộ chuyển động của các trục theo biến thời gian bằng bộ điều khiển trung tâm. Phần sau đây sẽ trình bày một phương pháp thết kế bộ điều khiển bám quỹ đạo cho các trục của tay máy. 2.2. Thiết kế bộ điều khiển bám cho các trục tay máy 2.2.1. Mô hình đối tượng điều khiển Mỗi trục tay máy được điều khiển bởi một hệ truyền động, bao gồm bộ điều khiển servo và động cơ servo (thuộc loại động cơ đồng bộ kích thích vĩnh cửu). Lựa chọn bộ điều khiển servo hoạt động ở chế độ điều khiển mô men, mô hình đối tượng điều khiển như hình 3 [2]: Hình 3. Mô hình đối tượng điều khiển. Trong đó: + Te * là mô men đặt cho Bộ điều khiển servo. + Te là mô men điện từ. + Td là mô men tải quy về đầu trục động cơ. + J là mô men quán tính. + b là hệ số ma sát quay. Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 47 + ω là tốc độ động cơ. 2.2.2. Xây dựng thuật toán điều khiển Từ yêu cầu đặc trưng của bài toán - yêu cầu của hệ truyền động chính xác cũng như hệ truyền động servo, đó là: hệ thống tác động nhanh nhưng không có quá chỉnh, không dao động trong thời kỳ quá độ. Nhóm tác giả lựa chọn bộ điều khiển PIV (bộ điều khiển PID có thay đổi cấu trúc) để điều khiển bám quỹ đạo cho các trục tay máy (vì với yêu cầu trên đối với hệ truyền động servo thì bộ điều khiển PIV tác động nhanh hơn nhiều bộ điều khiển PID [3]). Cấu trúc hệ điều khiển bám cho từng trục tay máy như hình 4. Trong đó: - Kp, Ki, Kv là các hệ số của bộ điều khiển PIV. - θ* và θ là giá trị đặt và giá trị phản hồi của góc quay mỗi trục. Các tham số Kp, Ki, Kv được tính toán và xác định tối ưu theo [2]. Hình 4. Cấu trúc hệ điều khiển bám cho từng trục tay máy. 2.3. Cấu trúc bộ điều khiển bám quỹ đạo đầu công tác Mỗi trục tay máy có một quỹ đạo chuyển động riêng và được điều khiển bám bởi một bộ điều khiển PIV như đã thiết kế ở phần trên. Công việc còn lại của bộ điều khiển trung tâm (bộ điều khiển bám quỹ đạo đầu công tác) là đồng bộ thời gian chuyển động cho tất cả 6 trục của tay máy, chính là biến thời gian trong đa thức xấp xỉ hàm của mỗi trục. Sơ đồ khối bộ điều khiển bám quỹ đạo đầu công tác tay máy 6 bậc tự do như sau: Hình 5. Sơ đồ khối bộ điều khiển bám quỹ đạo đầu công tác. 3. MÔ PHỎNG Thực hiện mô phỏng để kiểm chứng giải pháp điều khiển bám quỹ đạo của đầu công tác tay máy, cụ thể như sau: BỘ ĐIỀU KHIỂN TRUNG TÂM BỘ ĐIỀU KHIỂN TRUYỀN ĐỘNG TRỤC 1 BỘ ĐIỀU KHIỂN TRUYỀN ĐỘNG TRỤC 6 . . . . BỘ ĐIỀU KHIỂN TRUYỀN ĐỘNG TRỤC 2 Kỹ thuật điều khiển & Điện tử T. T. Tiến, P. M. Tân, , “Điều khiển bám quỹ đạo cho đầu công tác tay máy 6 bậc tự do.” 48 - Quỹ đạo chuyển động của mỗi trục tay máy đã được xấp xỉ thành 1 đa thức bậc cao theo thời gian t, biến thời gian này là chung cho cả 6 trục tay máy và được xây dựng dựa trên cơ sở số điểm rời rạc và tổng thời gian yêu cầu để tay máy di chuyển hết quỹ đạo. - Mô phỏng bộ điều khiển bám PIV cho mỗi trục tay máy theo thời gian t. - Thu được kết quả đầu ra của bộ điều khiển bám PIV chính là góc quay của từng trục. - Theo bước tính của biến thời gian t, tiếp tục giải bải toán động học thuận cho từng điểm ta được quỹ đạo chuyển động của đầu công tác thực tế (sau điều khiển). Chiếu quỹ đạo chuyển động thực tế ấy nên các mặt phẳng tọa độ để quan sát sai số của bài toán điều khiển. 3.1. Số liệu đầu vào - Mô phỏng hệ thống sử dụng Matlab & Simulink với bộ tham số thực tế của tay máy 6 bậc tự do Hitachi M6100 như sau: Bảng 1. Bảng tham số động học của tay máy M6100. TT θ d (m) a (m) α (0) θ0( 0) 1 q1 0.78 0 90 0 0 2 q2 0 0.7 0 90 0 3 q3 0 0.075 90 0 0 4 q4 0.95 0 -90 0 0 5 q5 0 0 90 0 0 6 q6 0.35 0 0 0 Trong đó: + qi là biến góc quay của trục thứ i (i = 1÷6). + d, a, α, θ0 là các tham số động học của tay máy. Bảng 2. Bảng tham số truyền động của tay máy M6100. TT θ P (W) G Jdc (kg.m2) Jd (kg.m2) Tmax (Nm) Tpulse (Nm) Km (Nm/V) 1 q1 280 100 0,32.10 -4 49,08.10-4 1,32 3,96 0,144 2 q2 540 132 1,93.10 -4 19,14.10-4 1,91 5,73 0,207 3 q3 540 132 1,93.10 -4 10,99.10-4 1,91 5,73 0,207 4 q4 130 53 0,19.10 -4 4,2.10-4 0,69 2,07 0,573 5 q5 130 60 0,19.10 -4 4,8.10-4 0,69 2,07 0,573 6 q6 90 50 0,12.10 -4 3,4.10-4 0,48 1,44 0,396 Trong đó: + P là công suất động cơ. + G là tỉ số truyền hộp số. + Jdc là mô men quán tính động cơ. + Jd là mô men quán tính tải. + Tmax là mô men tối đa của động cơ. + Tpulse là mô men quá tải ngắn hạn. + Km là hệ số khuếch đại khâu điều chỉnh mô men. - Mô phỏng với hệ số ma sát tải b = 10-3 Nm/(rad/s) và tải thay đổi đột ngột có giá trị Td = 100% tải tác động đồng thời vào các động cơ của mỗi trục tay máy tại thời điểm t = 2s. - Mô phỏng quỹ đạo chuyển động của tay máy là đường tròn trên mặt phẳng song song với mặt phẳng YOZ có bán kính R = 100mm và tọa độ tâm là (1335,0,1573) trong hệ trục tọa độ gốc của tay máy. Phương trình quỹ đạo chuyển động: Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 49 Trong đó: (Ex, Ey, Ez) là tọa độ đầu công tác tay máy. - Rời rạc hóa phương trình chuyển động theo thời gian, chọn góc tiếp cận của đầu công tác vuông góc với mặt phẳng chứa quỹ đạo và hướng tiếp cận tiếp tuyến với quỹ đạo. - Áp dụng phương pháp bình phương tối thiểu, xây dựng được các phương trình chuyển động của các trục là các đa thức bậc 9 theo thời gian như sau: Bảng 3. Bảng hệ số phương trình chuyển động của các trục. a9 a8 a7 a6 a5 a4 a3 a2 a1 a0 q1 4.09E-05 -0.00174 0.03089 -0.29915 1.740681 -6.55161 17.19237 -29.9182 23.56416 -2.76358 q2 -5.47E-09 1.73E-05 -0.00065 0.009721 -0.07425 0.311144 -0.80722 1.686253 -2.38998 90.32064 q3 2.23E-08 -0.00013 0.005055 -0.07444 0.534819 -1.95266 4.09279 -8.83583 15.26582 -3.10419 q4 0.000324 -0.01376 0.251463 -2.59275 16.56381 -67.7686 176.9868 -283.647 309.8084 -365.083 q5 3.48E-08 -3.83E-05 0.001417 -0.02276 0.205482 -1.13402 3.893635 -8.04528 9.013141 1.922128 q6 -2.42E-04 1.03E-02 -0.18867 1.965938 -12.7679 53.38652 -142.956 234.8468 -213.324 261.3576 Trong đó: ai (i=0÷9) là các hệ số của đa thức bậc 9. 3.2. Kết quả mô phỏng bám quỹ đạo của các trục Hình 6. Đồ thị bám quỹ đạo từ trục 1 đến trục 6. 0 2 4 6 8 10 -8 -6 -4 -2 0 2 4 6 8 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 88.8 89 89.2 89.4 89.6 89.8 90 90.2 90.4 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 -8 -6 -4 -2 0 2 4 6 8 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 -200 -150 -100 -50 0 50 100 150 200 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 5.99 6 6.01 6.02 6.03 6.04 6.05 6.06 6.07 6.08 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 179.7 179.8 179.9 180 180.1 180.2 180.3 Net dut: Dau vao; Net lien: Dau ra Time (s) Kỹ thuật điều khiển & Điện tử T. T. Tiến, P. M. Tân, , “Điều khiển bám quỹ đạo cho đầu công tác tay máy 6 bậc tự do.” 50 Hình 7a. Đồ thị sai số bám quỹ đạo các trục 1, 2, 3 và 4. Hình 7b. Đồ thị sai số bám quỹ đạo các trục 5 và 6. - Nhận xét: Với cấu trúc hệ thống đã nêu khi sử dụng bộ điều khiển PIV, tất cả các trục đã bám tốt với các quỹ đạo chuyển động phức tạp khác nhau của chúng. Kết quả cụ thể như sau: Bảng 4. Bảng kết quả bám quỹ đạo của các trục. q1 q2 q3 q4 q5 q6 Sai số bám 0.0030 0.00020 0.0010 0.0010 0.0010 0.0010 Sai số khi quá độ 0.060 0.00240 0.0040 0.40 0.010 0.0020 - Sai số bám của các trục là rất tốt. Tuy nhiên, sai số khi quá độ vẫn khá lớn, đặc biệt là ở trục 1 và trục 4 do ảnh hưởng của cơ cấu cơ khí và đặc tính chuyển động của mỗi trục, điều này có thể cải thiện bằng các bộ tạo tín hiệu đặt dạng hình thang trong quá trình khởi động và dừng chuyển động khi áp dụng trong thực tế. - Sai số này cùng với sai số do phép xấp xỉ hàm bằng phương pháp bình phương tối thiểu ở trên tạo nên sai số quỹ đạo chuyển động của đầu công tác tay máy và được thể hiện ở phần dưới đây. 3.3. Kết quả mô phỏng bám quỹ đạo của đầu công tác của tay máy 0 2 4 6 8 10 -2 -1 0 1 2 3 4 x 10 -3 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 -0.06 -0.04 -0.02 0 0.02 0.04 0.06 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 1 2 3 4 5 6 7 -5 0 5 10 15 20 25 x 10 -4 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 -5 0 5 10 15 20 25 x 10 -3 Net dut: Dau vao; Net lien: Dau ra Time (s) 0 2 4 6 8 10 -3 -2 -1 0 1 2 3 x 10 -3 Net dut: Dau vao; Net lien: Dau ra Time (s) Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 51 Từ kết quả bám quỹ đạo của từng trục, giải bài toán động học thuận để xây dựng quỹ đạo chuyển động của đầu công tác tay máy sau khi xấp xỉ hàm và điều khiển bám theo quỹ đạo đã xấp xỉ. Chiếu quỹ đạo chuyển động lên các mặt phẳng công tác để xác định sai số quỹ đạo. Hình 8. Hình chiếu quỹ đạo chuyển động lên mp(YOZ), mp(XOY), mp(XOZ). Nhận xét: - Sai số lớn nhất trong quá trình bám quỹ đạo: 0.01mm. - Sai số lớn nhất trong thời kỳ quá độ và khi có nhiễu tải: 0.015mm. - Sai số lớn nhất khi có mô men tải tác động vào tất cả các trục: 0.025mm. 4. KẾT LUẬN Bài viết đã đưa ra một giải pháp cho bài toán điều khiển tay máy 6 bậc tự do di chuyển theo quỹ đạo liên tục cho trước trong không gian. Với kết quả mô phỏng ở trên đã chứng minh giải pháp đưa ra để điều khiển đầu công tác bám theo quỹ đạo liên tục trong không gian là đúng đắn, có thể áp dụng tốt cho các ứng dụng thực tế. Nội dung trình bày trên nằm trong khuôn khổ Đề tài nghiên cứu khoa học cấp Viện KH&CN quân sự “Nghiên cứu, thiết kế chế tạo hệ thống nhúng chuyên dụng điều khiển cơ cấu tay máy 6 trục tự do định hướng ứng dụng trong dây chuyền sản xuất”. Các kết quả trình bày ở trên đã được cài đặt và chạy thử nghiệm ổn định trên tay máy Hitachi M6100. TÀI LIỆU THAM KHẢO [1]. Trần Trung Kiên, Đặng Nam Kiên, Vũ Đức Tuấn, Nguyễn Duy Trung, “Giải bài toán động học cho robot 6 bậc tự do Hitachi M6100 bằng phương pháp phân đoạn,” TC. Nghiên cứu KHCNQS, số Đặc san (04-2014), tr. 63-69. [2]. Phạm Minh Tân, “Nghiên cứu bộ điều khiển truyền động Delta cho động cơ đồng bộ 3 pha và ứng dụng để thiết kế chế tạo hệ điều khiển bám,” Luận văn tốt nghiệp cao học – Học Viện Kỹ thuật quân sự (2013), tr. 55-60. [3]. Kaiser D, Fundamentals of Servo Motion Control, Parker Compumotor, (2007). [4]. Ferenc Lombai, Gábor Szederkenyi, “Trajectory tracking control of a 6 degree-of- freedom robot arm using nonlinear optimization,” IEEE (2008). [5]. S.Yauldegar, Hootan Ghiasi, Mohammad Hadi Mazloom, Amirzubir Sahamijoo, Mohammad Reza Avazpour, Farzin Piltan, “Trajectory Tracking Control of Multi Degrees of Freedom Joints: Robust Fuzzy Logic-Based Sliding Mode Approach,” International Journal of Control and Automation, Vol. 7, No. 12 (2014), pp. 323-338 [6]. Himanshu Chaudhary, Vikas Panwar, N. Sukavanam, and Rajendra Prasad, “ANFIS PD+I Based Hybrid Force/ Position Control of an Industrial Robot Manipulator,” International Journal of Materials, Mechanics and Manufacturing, Vol. 2, No. 2, May 2014 [7]. Yavuz Sarı, “Performance Evaluation of the Various Training Algorithms and Network Topologies in a Neural-network-based Inverse Kinematics Solution for Robots,” Hình chiếu quỹ đạo tròn và đầu công tác Hình chiếu quỹ đạo tròn Hình chiếu quỹ đạo đầu công tác Hình chiếu quỹ đạo tròn Hình chiếu quỹ đạo đầu công tác Kỹ thuật điều khiển & Điện tử T. T. Tiến, P. M. Tân, , “Điều khiển bám quỹ đạo cho đầu công tác tay máy 6 bậc tự do.” 52 International Journal of Advanced Robotic Systems (2014). ABSTRACT TRAJECTORY TRACKING CONTROL OF A 6-DOF MANIPULATOR 6-DOF manipulator is becoming more and more popular with requiring a higher accuracy. The accuracy is required not only in a point but also in a trajectory. A trajectory tracking control method of a 6-dof manipulator is presented in this paper. Keywords: 6-dof manipulator, Trajectory, Tracking control. Nhận bài ngày 18 tháng 01 năm 2017 Hoàn thiện ngày 16 tháng 6 năm 2017 Chấp nhận đăng ngày 20 tháng 6 năm 2017 Địa chỉ: Viện Tự động hóa KTQS, Viện KH&CN quân sự. *Email: trantien315@gmail.com