ĐIỀU KHIỂN ROBOT CÔNG NGHIỆP 7 BẬC TỰ DO - Phân tích thiết kế và mô phỏng trên phần mềm Matlab
Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 1 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) ĐIỀU KHIỂN ROBOT CÔNG NGHIỆP 7 BẬC TỰ DO Phân tích thiết kế và mô phỏng trên phần mềm Matlab 1. Đặt vấn đề Trong quá trình thiết kế robot công nghiệp, thì việc lựa chọn động cơ có đủ mô men không hề đơn giản. Nếu chọn động cơ có mô men quá lớn thì kích thước và khối lượng của nó cũng lớn. Ngược lại nếu chọn động cơ có mô men nhỏ thì robot có thể không hoạt động được ở một số không gian làm việc nào đó hoặc không đạt được vận tốc, gia tốc mong muốn. Việc tính toán mô men cần thiết của động cơ có thể dựa vào các phần mềm động lực học. Một cách khác, là thông qua việc mô phỏng thuật toán điều khiển trong không gian làm việc của robot với các vận tốc và gia tốc mong muốn (đạt được giá trị lớn nhất). Tuy nhiên, nhiệm vụ chính trong phần mô phỏng hệ thống điều khiển là tìm ra qui luật biến thiên của lỗi khi thay đổi đầu vào và các tham số điều khiển. Qua đó sẽ xác định được tham số điều khiển tốt nhất để cài đặt xuống bộ điều khiển thực tế. Đối với các robot công nghiệp có ít bậc tự do (2 hoặc 3), thì việc mô phỏng hệ thống điều khiển có thể thực hiện trên Simulink. Trong những trường hợp này, việc thiết kế các bộ điều khiển tương đối dễ dàng. Kỹ sư thiết kế điều khiển chỉ việc gắp các khối trong Simulink và chèn vào cửa sổ màn hình của chương trình. Việc thiết kế như vậy tuy đơn giản nhưng khi chạy chương trình mô phỏng thì thường gặp nhiều rắc rối. Thứ nhất, việc giải phương trình động lực học trên simulink thường có lỗi ở các khối tích phân (tính vận tốc bằng cách tích phân gia tốc, tính vị trí bằng cách tích phân vận tốc). Thứ hai, khi phương trình động lực học phức tạp, việc chạy mô phỏng rất tốn thời gian và nhiều khi không thực hiện được. Do vậy, tài liệu tham khảo này sẽ trình bày một giải pháp khác là thiết kế bộ điều khiển để chạy mô phỏng trên m file của Matlab thay vì chạy trên Simulnk. Giải pháp này có thể áp dụng cho robot công nghiệp với hầu hết các loại bậc tự do. Để minh họa rõ hơn cho ưu điểm của cách này so với cách chạy trên Simulink, robot công nghiệp 7 bậc tự do sẽ được lựa chọn để mô phỏng. Cả hai bộ điều khiển vị trí và vận tốc cùng được phân tích và thiết kế. Tuy nhiên, thực tế chỉ cần dùng bộ điều khiển vị trí là đủ để điều khiển bám quĩ đạo. Khi có thêm phần điều khiển tốc độ thì sai lệch vị trí có được cải thiện đôi chút nhưng nó cũng gây lên sự phức tạp cho việc cài đặt thuật toán điều khiển xuống bộ vi xử lý. 2. Thiết kế hệ thống điều khiển Hình 1 dưới đây miêu tả sơ đồ cấu trúc của hệ thống điều khiển robot công nghiệp. Ở đó, bộ điều khiển PID sẽ được lựa chọn để mô phỏng. Phần mô hình của robot bao gồm khối động lực học, khối động cơ, khối khuếch đại công suất và khối lọc. Tất cả các khối này sẽ được mô hình hóa để thiết kế một hệ thống điều khiển đầy đủ như chạy trong thực tế. Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 2 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Hình 1 Sơ đồ mô phỏng hệ thống điều khiển robot công nghiệp Ở trên hình 1, khối trajectory planning có chức năng chuyển quĩ đạo từ hệ tọa độ đề các sang các góc quay của khớp (giải động học ngược). Đầu ra của bộ điều khiển PID là giá trị điện áp sẽ được lọc để giảm nhiễu trước khi tới bộ khuếch đại công suất (nếu dùng động cơ một chiều thì nó là Driver, nếu dùng động cơ xoay chiều thì là biến tần Inverter). Đầu ra của khối khuếch đại công suất này sẽ cung cấp dòng đủ lớn cho động cơ. Dòng điện chạy trong các cuộn dây của động cơ sẽ sinh ra mô men trên trục được lắp với bộ truyền động để tăng mô men cung cấp cho các khâu của robot. 2.1 Mô hình hóa Phương trình động lực học tổng quát của robot có dạng như sau; ( ) ( , ) ( )M q q V q q G q t + + = && & (1) ở đó, ( ) M q là ma trận n n ´ với n là số khớp của robot. Ma trận này chứa các tham số khối lượng và mô men quán tính. ( , ) V q q & là ma trận 1 n ´ miêu tả ảnh hưởng qua lại giữa vận tốc góc của các khớp. ( ) G q là ma trận 1 n ´ miêu tả ảnh hưởng của trọng lực. Từ phương trình động lực học này, véc tơ gia tốc của các khớp có thể được tính là, 1 1 1 ( , ) ( )q M V q q M G q M t - - - = - - + && & (2) ở đó, t là mô men quay quanh các khớp, được cung cấp bởi đầu ra của bộ truyền động và có dạng hàm vi phân bậc nhất dưới đây, 1 1 m g g t t at t t = - + & (3) Gear Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 3 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) với, a là tỉ số truyền, g t là hằng số thời gian của bộ truyền, và m t là mô men của động cơ, có quan hệ với hằng số mô men động cơ t K và dòng điện i như sau, m t K i t = (4) Dòng điện chạy trong động cơ được cung cấp từ bộ khuếch đại công suất cũng có dạng hàm vi phân bậc nhất là, p pa i i K V t + = & (5) ở đó, V là điện áp đầu vào (chính là giá trị đầu ra của bộ điều khiển), p t là hằng số thời gian của bộ khuếch đại công suất và pa K là hệ số khuếch đại. Kết hợp giữa mô hình của động cơ và bộ khuếch đại công suất, thu được 1 1 m m t pa p p K K V t t t t = - + & (6) Như vậy, các thành phần của hệ thống robot đã được mô hình hóa. Bước tiếp theo là thiết kế bộ điều khiển PID dưới dạng tổng quát, rồi sau đó áp dụng cho điều khiển vị trí và vận tốc của các khâu robot. 2.2 Thiết kế bộ điều khiển PID Hàm truyền của bộ điều khiển PID với bộ lọc bậc nhất được biểu diễn như sau, 2 2 ( ) ( ) ( ) 1 ( ) p d i i p d f f f f f K K K K s s K K s V s V s s s E s s E s s t t t t t + + + + = ® = + + (7) ở đó, E là sai lệch giữa giá trị mong muốn và phản hồi, V là điện áp, từ đầu ra của bộ điều khiển, , , p i d K K K là hệ số tỉ lệ, tích phân và vi phân, f t là hằng số thời gian bộ lọc Sơ đồ khối của bộ điều khiển PID cùng với bộ lọc bậc nhất được thể hiện trên sơ đồ sau. Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 4 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Hình 2 Sơ đồ khối của bộ điều khiển PID và bộ lọc Từ sơ đồ khối này, chúng ta có, 1 1 2 1 2 1 1 1 2 2 1 f p p i d d i d f f f f f f f x x e x x K K K K K K K V x x x x x e t t t t t t t t = - + = æ ö = + + = - + + ç ÷ ç ÷ è ø & & & (8) Viết gọn các phương trình này dưới dạng không gian trạng thái như sau, 1 1 2 2 1 2 2 1 0 1 0 1 0 f p d i d f f f f x x e x x K x K K K V e x t t t t t é ù - é ù é ù é ù ê ú = + ê ú ê ú ê ú ê ú ë û ë û ë û ê ú ë û é ù é ù = - + ê ú ê ú ê ú ë û ë û & & (9) đặt 1 f t 1 s 1 s 1 f t d f K t d f K t + V E - V && V & V 1 x & 1 x 2 x & 2 x + + + + + + + Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 5 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) 1 1 2 2 2 1 0 1 0 1 0 p d i d f f f f f x x X X x x K K K K A B C D t t t t t é ù é ù = = ê ú ê ú ë û ë û é ù - é ù é ù ê ú = = = - = ê ú ê ú ê ú ê úë û ë û ê ú ë û & & & dẫn tới, X AX Be V CX De = + = + & (10) Đây là phương trình của bộ điều khiển PID và bộ lọc dưới dạng không gian trạng thái. Áp dụng cho điều khiển vị trí Giả sử, sai lệch vị trí của các khớp là d e q q = - , ở đó, vị trí mong muốn là d q và phản hồi là q . Do vậy, phương trình không gian trạng thái điều khiển vị trí sẽ là; 1 1 1 1 1 1 1 1 1 1 d d X A X B q B q V C X D q D q = - + = - + & (11) với, 1 1 1 1 1 1 1 1 2 1 0 1 0 1 0 p d i d f f f f f K K K K A B C D t t t t t é ù - é ù é ù ê ú = = = - = ê ú ê ú ê ú ê úë û ë û ê ú ë û 1 1 1 , , p i d K K K là hệ số tỉ lệ, tích phân và vi phân, ứng với bộ PID điều khiển vị trí. Áp dụng cho điều khiên vận tốc Giả sử sai lệch vận tốc của các khớp là v d e q q = - & & , ở đó, vận tốc mong muốn là d q & và phản hồi là q & . Vì vậy, phương trình không gian trạng thái điều khiển vận tốc là; 2 2 2 2 2 2 2 2 2 2 d d X A X B q B q V C X D q D q = - + = - + & & & & & (12) ở đó 2 2 2 2 2 2 2 2 2 1 0 1 0 1 0 p d i d f f f f f K K K K A B C D t t t t t é ù - é ù é ù ê ú = = = - = ê ú ê ú ê ú ê ú ë û ë û ê ú ë û Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 6 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) 2 2 2 , , p i d K K K là hệ số tỉ lệ, tích phân và vi phân ứng với bộ PID điều khiển tốc độ. Như vậy, tổng điện áp từ hai bộ điều khiển tới bộ khuếch đại công suất là, 1 2 V V V = + và mô men của động cơ được tính như sau, ( ) 1 2 1 1 m m t pa p p K K V V t t t t = - + + & (13) Tổng hợp các phương trình trên, thu được hệ phương dưới đây, ( ) 1 1 1 1 2 1 1 2 2 1 2 1 2 1 1 1 1 1 2 2 2 2 2 ( , ) ( ) 1 1 1 1 1 1 1 1 1 1 1 m g g m m t pa p p m t pa t pa p p p t pa t pa p p t pa d t pa d p p d d q M V q q M G q M q q K K V V K K C X K K C X K K D q K K D q K K D q K K D q X A X B q B q X A X B q B q t t t at t t t t t t t t t t t t t t - - - ì = - - + ï ï = ï ï = - + ï = - + + = - + + í - - + + = - + = - + && & & & & & & & & & & & ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï î (14) Định dạng lại hệ phương trình này như sau;. Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 7 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) 1 2 1 1 2 1 1 2 1 1 2 2 2 1 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 ( , ) g g m m t pa t pa t pa t pa p p p p p M q q I q q K K D K K D K K C K K C X X X B A X B A M V q q M G a t t t t t t t t t t t - - - é ù ê ú é ù é ù ê ú ê ú ê ú ê ú - ê ú ê ú ê ú ê ú ê ú ê ú =ê ú ê ú ê ú ê ú ê ú ê ú - - - ê ú ê ú ê ú ê ú ê ú ê ú ê ú - ê ú ë û ë û ê ú ê ú - ë û - - + && & & & & & & & 1 2 1 2 ( ) 0 0 1 1 t pa d t pa d p p d d q K K D q K K D q B q B q t t é ù ê ú ê ú ê ú ê ú ê ú + ê ú ê ú ê ú ê ú ë û & & (15) Đây là phương trình vi phân bậc nhất dạng X AX Bu = + & nên có thể giải dễ dàng trên Matlab (xem hướng dẫn cách giải ở phần cuối của tài liệu này). 3. Mô phỏng trên phần mềm MATLAB Mô hình 3D của một loại robot công nghiệp 7 bậc tự do, sử dụng trong phần mô phỏng này, được miêu tả như trên hình dưới đây. Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 8 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Hình 3 Mô hình 3D của một loại robot công nghiệp 7 bậc tự do Robot công nghiệp này có 6 khớp quay và 2 khớp tịnh tiến nhưng do khớp 2 và khớp 4 có ràng buộc với nhau để cho khớp tịnh tiến 5 luôn song song với mặt đất, nên robot này chỉ có 7 bậc tự do. Cấu trúc robot hơi phức tạp nhưng với 2 khớp tịnh tiến nó có thể thu nhỏ cấu hình, mở rộng không gian làm việc và trở lên linh hoạt hơn. Robot loại này sẽ có nhiều ứng dụng trong thực tế sản xuất như việc lắp ráp 2 chi tiết máy trong điều kiện nhiệt độ cao. Các hệ tọa độ của robot được định nghĩa như trên hình 4. Khớp quay 1 Khớp quay 2 Khớp tịnh tiến 3 Khớp quay 4 Khớp tịnh tiến 5 Khớp quay 6, 7, 8 Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 9 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Hình 4 Hệ trục tọa độ của robot công nghiệp 7 bậc tự do Các tham số DH được miêu tả dưới bảng sau: Axis a i α i d i Θ i Initial value 1 a 1 (0.22m) 90 o d 1 (0.420m) Θ 1 90 o 2 0 -90 o 0 Θ 2 0 3 0 90 o d 3 (variable) 0 1.025 4 0 0 d 4 (variable) 0 0.5645 5 a 4 (0.207m) 90 o 0 Θ 5 = (90 o -Θ 2 ) 90 o 6 0 -90 o 0 Θ 6 0 7 0 90 o 0 Θ 7 0 8 0 0 d 8 (0.018m) Θ 8 0 Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com 10 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Các tham số của robot 7 bậc tự do được cho bên dưới. 1 4 1 8 1 2 3 4 5 6 7 8 2 0.2; 0.2; 0.475; 0.093 ( ) 85.058; 62.157; 37.636; 55 ( ) 84.165; 14.661; 6.591; 2.358 ( ) I1x = 1.620; I1y = 1.595; I1z = 1.077; ( kgm ) I2x = 3.556; I2y = 0.458; I2z = 3.572; (k a a d d m m m m m kg m m m m kg = = = = = = = = = = = = 2 2 2 2 2 gm ) I3x = 1.257; I3y = 1.292; I3z = 0.119; (kgm ) I4x = 1.500; I4y = 1.500; I4z = 1.500; ( kgm ) I5x = 6.170; I5y = 6.225; I5z = 0.274; (kgm ) I6x = 0.104; I6y = 0.086; I6z = 0.039; ( kgm ) I7x = 0.007; I 2 2 2 7y = 0.007; I7z = 0.005; (kgm ) I8x = 0.0003; I8y = 0.0003; I8z = 0.0005 ; (kgm ) r1 = 0.15; r2 = 0.296; r3 = 0.736; r4 = 0.1; (m) r5 = 0.4; r6 = 0.7; r7 = 0.01; r8 = 0.086; (m) m gc = 9.81 ; s t0=atan(4 æ ö ç ÷ è ø 75/200); Tham số mô hình của bộ lọc, động cơ (thường dùng động cơ xoay chiều vì nó có kích thước nhỏ gọn và mô men lớn) và bộ truyền động (giả sử hiệu suất bằng 100%), được chọn là: 1 2 3 4 5 6 7 8 1 1 1 ; ; 30000 30000 30000 1; 1; : 56; 56; 500; 56; 500; 1; 1; 1; p g f pa t K K Gear ratio t t t a a a a a a a a = = = = = = = = = = = = = Tham số của bộ PID điều khiển vị trí được lựa chọn sau nhiều lần mô phỏng là: [...]... liệu này đã trình bày một giải pháp thiết kế và mô phỏng bộ điều khiển PID cho robot công nghiệp 7 bậc tự do Các thành phần của hệ thống robot đã được mô hình hóa để kết quả mô phỏng gần với kết quả thực nghiệm Tiến sĩ Vũ Minh Hùng cũng đã cài đặt các thuật toán điều khiển cho robot này tại một công ty ở Hàn Quốc Trong thời gian tới, các sản phẩm robot có cấu trúc loại này sẽ xuất hiện trên thị trường... 0.005 K i = 0.01 Join 7 : K p = 70 K d = 0.009 Ki = 0.5 Join 8 : K p = 0.01 K d = 0.0001 K i = 0.01 và của bộ PID điều khiển vận tốc là: Join 1: K p = 50 K d = 0.0005 Ki = 0.5 Join 2 : K p = 10000 K d = 0.03 K i = 0.5 Join 3 : K p = 4000 K d = 0.02 Ki = 0.5 Join 4 : K p = 4000 K d = 0.02 Ki = 0.5 Join 5 : K p = 100 K d = 0.0005 K i = 0.5 Join 6 : K p = 50 K d = 0.0005 K i = 0.01 Join 7 : K p = 25 K d =... 25 K d = 0.0009 Ki = 0.5 Join 8 : K p = 0.1 K d = 0.0001 K i = 0.01 Quĩ đạo của robot được thiết kể để chạy đến tốc độ tối đa 1.5 m/s tại gốc cái tay kẹp (end effector) Thời gian mô phỏng ngắn (chỉ có 3 giây) để robot có thể đạt được gia tốc rất lớn Đồ thị vận tốc của robot được biểu diễn trên hình 5 Hình 5 Vận tốc của robot tại điểm gốc của tay kẹp (end effector) 11 Diễn đàn Cơ điện tử, Haptic và... thấy rằng robot đã bám rất tốt quĩ đạo thiết kế Tuy nhiên tại thời điểm gia tốc lớn thì sai lệch vị trí cũng tăng theo Hình 6 So sánh vị trí của các khớp 12 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com Hình 7 bên dưới là sự so sánh giữa vận tốc mong muốn và phản hồi Ở đó cũng thể hiện rằng sai lệch vận tốc lớn khi gia tốc tăng Hình 7 So sánh... trong thực tế Hướng dẫn giải phương trình không gian trạng thái bên trên để có được kết quả mô phỏng Đặt giá trị khởi đầu cho các véc tơ % initial values for solving equations q_init= [0 0 0.854 1. 570 8 0. 874 0 0 0 0]'; % initial values of joint position qd_init= zeros(8,1); % initial values of joint velotiy torque= zeros(8,1); % initial values of joint torque torque_m= zeros(8,1); % initial values of... hàm ode23 trong matlab [t,w]= ode23('cal_dynamics_PID', tspan, winit); % Using ode23 to solve state space equations % subfunction for cal_dynamics_PID function wdot = cal_dynamics_PID(t,w) insert dynamics equation and other equations here wdot = ; t k= fix(t/delt)+1; Các bạn quan tâm có thể liên hệ với chúng tôi theo địa chỉ email: vmhung2010@gmail.com để có được file chương trình mô phỏng đầy đủ... kiểm tra khả năng của động cơ Hình 8 Gia tốc của các khớp 14 Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Tiến sĩ Vũ Minh Hùng http://matlabs.wordpress.com Hình 9 là đồ thị mô men động cơ để robot có thể hoạt động được với quĩ đạo bên trên Tuy nhiên để ước lượng chính xác mô men lớn nhất của động cơ thì cần phải mô phỏng cho các không gian làm việc khác, và tính cụ thể cho một số điểm đặc