Điều khiển phi tuyến hệ thống tay máy đôi

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LUẬN VĂN THẠC SĨ Điều khiển phi tuyến hệ thống tay máy đôi VŨ HỮU TỈNH Ngành: Kỹ thuật điều khiển tự động hóa Giảng viên hướng dẫn: TS Nguyễn Tùng Lâm Viện: Điện HÀ NỘI, 2020 TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LUẬN VĂN THẠC SĨ Điều khiển phi tuyến hệ thống tay máy đôi VŨ HỮU TỈNH Ngành: Kỹ thuật điều khiển tự động hóa Giảng viên hướng dẫn: TS Nguyễn Tùng Lâm Chữ ký GVHD Viện: Điện HÀ NỘI, 2020 CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc BẢN XÁC NHẬN CHỈNH SỬA LUẬN VĂN THẠC SĨ Họ tên tác giả luận văn: Vũ Hữu Tỉnh Đề tài luận văn: Điều khiển phi tuyến hệ thống tay máy đôi Chuyên ngành: Kỹ thuật điều khiển tự động hóa Mã số HV: CB180127 Tác giả, Người hướng dẫn khoa học Hội đồng chấm luận văn xác nhận tác giả sửa chữa, bổ sung luận văn theo biên họp Hội đồng ngày 30/10/2020 với nội dung sau: - Bổ sung lời mở đầu cho chương - Bổ sung kết luận sau chương - Chỉnh sửa lưu đồ thuật tốn thể mơ hình điều khiển - Chỉnh sửa thứ nguyên đồ thị kết mô Hà Nội, ngày Giáo viên hướng dẫn tháng năm 2020 Tác giả luận văn CHỦ TỊCH HỘI ĐỒNG Lời cảm ơn Tác giả xin gửi lời cảm ơn sâu sắc đến thầy hướng dẫn TS Nguyễn Tùng Lâm chịu trách nhiệm, định hướng đưa góp ý để tác giả hồn thành luận văn Tác giả xin cảm ơn tới Nhà trường, Viện Điện, thầy cô tham gia giảng dạy bạn đồng niên khóa cao học 2018B tạo điều kiện cho tác giả học tập nghiên cứu trường Đại học Bách Khoa Hà Nội Học viên Vũ Hữu Tỉnh Tóm tắt nội dung luận văn Trong xu hướng phát triển nhanh chóng khoa học cơng nghệ, tay máy đơi với ưu khả linh hoạt xử lý xác áp dụng nhiều lĩnh vực khác Việc xây dựng phương pháp điều khiển cho tay máy đôi liên tục phát triển, cải tiến Tuy nhiên, làm việc môi trường thực tế tay máy đôi chịu ảnh hưởng tác động tải, nhiễu bất định, hạn chế cấu phần cứng dẫn đến gây ổn định q trình điều khiển Chính thế, tác giả lựa chọn đề tài “Điều khiển phi tuyến hệ thống tay máy đôi” đưa thuật toán điều khiển phi tuyến bền vững nhằm khắc phục vấn đề Thông qua sở lý thuyết, mơ hình đối tượng điều khiển xây dựng mô tả đầy đủ đặc tính đối tượng Từ thực kiểm nghiệm môi trường mô thiết bị xử lý ngoại vi để xem xét, đánh giá chất lượng thuật toán điều khiển Các kết thu q trình mơ cho thấy khả thích nghi tốt điều khiển giải vấn đề đặt Tính khả thi cao điều khiển đươc kiểm chứng qua thiết bị xử lý ngoại vi tạo tiền đề thực kiểm nghiệm nâng cao trước đưa thực tiễn MỤC LỤC MỤC LỤC i CHƯƠNG TỔNG QUAN 1.1 Giới thiệu chung hệ thống tay máy đôi 1.2 Ứng dụng hệ thống tay máy đôi xu hướng nghiên cứu 1.3 Các nội dung nghiên cứu luận văn CHƯƠNG MƠ HÌNH HĨA ĐỐI TƯỢNG 2.1 Khảo sát hệ tay máy đôi mang tải 2.1.1 Động học thuận robot 2.1.2 Động học nghịch robot 2.2 Mơ hình hóa đối tượng điều khiển 2.2.1 Lực tương tác robot tay máy đôi tải 2.2.2 Phương trình động lực học robot tay máy đôi 12 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN CHO ĐỐI TƯỢNG 18 3.1 Cơ sở lý thuyết điều khiển 18 3.1.1 Điều khiển trượt 18 3.1.2 Điều khiển trượt siêu xoắn 21 3.1.3 Điều khiển trượt đầu cuối nhanh 24 3.2 Thiết kế điều khiển cho đối tượng tay máy đôi tổng quát 29 3.2.1 Bộ điều khiển trượt 30 3.2.2 Bộ điều khiển trượt siêu xoắn 31 3.2.3 Bộ điều khiển trượt đầu cuối nhanh 32 CHƯƠNG ỨNG DỤNG ĐIỀU KHIỂN ĐỐI TƯỢNG TAY MÁY ĐÔI ĐỒNG PHẲNG BẬC TỰ DO 35 4.1 Mơ hình hóa đối tượng 35 4.1.1 Phương trình động học thuận robot 36 4.1.2 Phương trình động học nghịch robot 37 4.1.3 Phương trình động lực học robot 38 4.2 Thiết kế điều khiển cho đối tượng 41 4.2.1 Bộ điều khiển trượt 41 4.2.2 Bộ điều khiển trượt siêu xoắn 42 4.2.3 Bộ điều khiển trượt đầu cuối nhanh 42 i 4.3 Mô kiểm chứng 43 4.3.1 Xây dựng quỹ đạo chuyển động tay robot 43 4.3.2 Kết mô 46 CHƯƠNG MÔ PHỎNG KIỂM CHỨNG PIL 57 5.1 Hướng tiếp cận thiết kế dựa mơ hình 57 5.2 Mô PIL điều khiển đối tượng tay máy đôi bậc tự 60 CHƯƠNG KẾT LUẬN 66 6.1 Kết luận 66 6.2 Hướng phát triển đề tài tương lai 66 TÀI LIỆU THAM KHẢO 67 PHỤ LỤC 69 ii DANH MỤC HÌNH VẼ Hình 1.1 Robot tay máy đơi cho trình xử lý, lắp ráp Hình 2.1 Cấu hình robot tay máy đôi Hình 2.2 Khung tọa độ cấu hình robot tay máy đơi Hình 2.3 Biểu diễn lực tương tác robot giữ tải cố định 10 Hình 2.4 Biểu diễn lực tương tác robot chuyển động mang tải 10 Hình 4.1 Mơ hình tay máy đôi đồng phẳng bậc tự 35 Hình 4.2 Giai đoạn chuyển động 43 Hình 4.3 Giai đoạn chuyển động 44 Hình 4.4 Giai đoạn chuyển động 44 Hình 4.5 Cấu trúc mô 46 Hình 4.6 Giá trị góc khớp điều khiển trượt 47 Hình 4.7 Giá trị góc khớp điều khiển trượt siêu xoắn 47 Hình 4.8 Giá trị góc khớp điều khiển trượt đầu cuối nhanh 48 Hình 4.9 Sai lệch giá trị góc khớp 48 Hình 4.10 Sự thay đổi góc tải khung tọa độ chung 49 Hình 4.11 Sai lệch góc tải 49 Hình 4.12 Mơ men điều khiển 50 Hình 4.13 Hiện tượng rung mơ men điều khiển 50 Hình 4.14 Lực tương tác robot tải 51 Hình 4.15 Hiện tượng rung lực tương tác 51 Hình 4.16 Góc khớp điểu khiển trượt có nhiễu 53 Hình 4.17 Góc khớp điều khiển trượt siêu xoắn có nhiễu 53 Hình 4.18 Góc khớp điều khiển trượt đầu cuối nhanh có nhiễu 53 Hình 4.19 Sai lệch góc khớp có nhiễu 54 Hình 4.20 Sai lệch góc xoay tải có nhiễu 54 Hình 4.21 Mơ men điều khiển có nhiễu 55 Hình 4.22 Lực tương tác robot tải có nhiễu 55 iii Hình 5.1 Chu trình thiết kế, kiểm tra cho đối tượng thực tế 57 Hình 5.2 Mơ MIL 58 Hình 5.3 Mơ SIL 59 Hình 5.4 Mô PIL 59 Hình 5.5 Mơ HIL 60 Hình 5.6 Mơ hình PIL chứa mã điều khiển 61 Hình 5.7 Bộ xử lý STM32F407 Discovery 61 Hình 5.8 Truyền thơng giao tiếp mô PIL 62 Hình 5.9 Kết nối xử lý với môi trường mô Simulink 62 Hình 5.10 Kiểm nghiệm quỹ đạo chuyển động 63 Hình 5.11 Kiểm nghiệm xoay tải 64 Hình 5.12 Kiểm nghiệm mơ men điều khiển 64 Hình 5.13 Kiểm nghiệm lực tương tác 65 iv DANH MỤC BẢNG BIỂU Bảng 2.1 Bảng thông số Denavit-Hartenberg (D-H) Bảng 4.1 Bảng thông số D-H cho robot tay máy đôi bậc tự 36 v CHƯƠNG TỔNG QUAN Hiện nay,trong bùng nổ cách mạng công nghiệp 4.0, xu hướng nghiên cứu phát triển robot nhận nhiều quan tâm trường đại học viện nghiên cứu Các robot thiết kế ngày tối ưu, tiết kiệm chi phí ứng dụng rộng rãi đời sống người, công nghiệp nhiều lĩnh vực khác xã hội Robot dần thay người để làm môi trường nguy hiểm hầm mỏ, mơi trường hóa chất độc hại, mơi trường phóng xạ, khơng có ngạc nhiên robot dùng việc vận chuyển uranium nhà máy nhiệt điện, hay xử lý vật liệu nổ… Với hiệu suất tính xác vượt trội so với người, phủ nhận rằng, robot yếu tố quan trọng việc cơng nghiệp hóa, đại hóa nâng cao suất lao động Khơng thế, với phát triển trí tuệ nhân tạo, robot ngày thiết kế có hình dạng, thao tác giống người, đáp ứng cầu cao xã hội Cũng giống người, thao tác robot thực nhờ cánh tay robot Do đó, hầu hết nghiên cứu phát triển robot thường tập trung vào phát triển cánh tay robot 1.1 Giới thiệu chung hệ thống tay máy đơi Hình 1.1 Robot tay máy đơi cho q trình xử lý, lắp ráp Robot song song hai cánh tay (Dual Arm Robot –DAR) có nhiều ưu điểm vượt trội so với dòng robot có cánh tay, ví dụ tải không Transactions on Cybernetics, vol 45, pp 507-508, March 2015 [11] N Yagiz, "Load transportation by dual arm robot using sliding mode control," Journal of Mechanical science and Technology, vol 24, no 5, pp 1177-1184, 2010 [12] L A Tuan, "Parameter estimator integrated-sliding mode control of dual arm robots," Int J.Control Autom Syst., vol 15, no 6, pp 2754-2763, 2017 [13] M W Spong, Robot Dynamic and control second edition, 2004 [14] N D Phước, "Điều khiển trượt trượt bậc cao," 2014 [15] X Wang and J Liu, Advanced Sliding Mode Control for Mechanical Systems: Design, Analysis and MATLAB Simulation, 2011 [16] A Levant, "Higher-order sliding modes, differentiation and output-feedback control," IJC, pp 924-941, 2003 [17] "MathWorks," [Online] Available: http://www.mathworks.com 68 PHỤ LỤC A1 Chi tiết số liệu thí nghiệm Các thơng số cấu hình hệ tay máy đôi đồng phẳng: = m1 5= ( kg ) , m2 4= ( kg ) , m3 5= ( kg ) , m4 ( kg ) = L1 0= ( m ) ,L2 0= ( m ) ,L3 0= ( m ) ,L4 0.4 ( m ) 0.2 ( m ) ,k3 0= k1 0= 25 ( m ) ,k2 = 25 ( m ) ,k4 0.2 ( m ) = = I1 = 0.1( kgm ) ; I 0= 08 ( kgm ) ; I = 0.1( kgm ) ; I 0.08 ( kgm ) = b1 100 = = = ( Nms ) , b2 100 ( Nms ) , b3 100 ( Nms ) , b4 100 ( Nms ) mload ( kg= 3; g 9.8 ( m s ) = ) ; µ 0= = d1 0= ( m ) ; d 0.4 ( m ) Thông số điều khiển trượt bản: λ 10 0   10 0  =  K  0 10     0 10   25 0   25 0     0 25     0 25 Thông số điều khiển trượt siêu xoắn λ  20 0   20 0  =  β  0 20     0 20  50 0   25 0     0 50     0 25 0  100  100 0   α=  0 100    0 100   Thông số điều khiển trượt đầu cuối nhanh: 69 = α 2= 0; β 1.0 = p 9= ; q0 = p 3= ;q lg = 3.0; η = 0.1 φ = 0.1; = γ q p s lg + η A2 Chi tiết sơ đồ mô Sơ đồ mô MIL giao diện phần mềm Matlab/Simulink Các thông số mô cài đặt dạng liệu tồn cục Mơ kiểm nghiệm thực hai chế độ có tác động nhiễu bên ngồi khơng có nhiễu tác động: 70 Mã code biểu diễn hàm quỹ đạo chuyển động: % function [Px1r,Py1r,Px2r,Py2r] = fcn(t) global L d1 d2 ml; %xf = [-0.1;-0.3]; yf = [0.7;0.7]; xf = [0.1;-0.1]; yf = [0.4;0.4]; qzero= [0;3*pi/4;pi;-3*pi/4]; xi= [d2/2+L(1)*cos(qzero(1))+L(2)*cos(qzero(1)+qzero(2)); -d2/2+L(3)*cos(qzero(3))+L(4)*cos(qzero(3)+qzero(4))]; yi= [L(1)*sin(qzero(1))+L(2)*sin(qzero(1)+qzero(2)); L(3)*sin(qzero(3))+L(4)*sin(qzero(3)+qzero(4))]; if t =2 d2xm = d1/2*(c5*dq5*dq5+sin(q5)*d2q5)(L(1)*(cos(q(1))*dq(1)*dq(1)+sin(q(1))*d2q(1))+L(2)*(cos(q(1)+q(2))*(d q(1)+dq(2))^2+sin(q(1)+q(2))*(d2q(1)+d2q(2)))); d2ym = d1/2*(s5*dq5*dq5-cos(q5)*d2q5)+ L(1)*(sin(q(1))*dq(1)*dq(1)+cos(q(1))*d2q(1))+L(2)*(sin(q(1)+q(2))*(dq(1)+dq(2))^2+cos(q(1)+q(2))*(d2q(1)+d2q(2))); Fs1 = ml*(d2ym*c5-d2xm*s5)/2+J*d2q5/d1; Fs2 = ml*(d2ym*c5-d2xm*s5)/2-J*d2q5/d1; D = -(ml*d2xm + (Fs1+Fs2)*s5)/c5; if (sqrt(Fs1^2+(ml*g/2)^2)/myu-D)>sqrt(Fs2^2+(ml*g/2)^2) F2 = sqrt(Fs1^2+(ml*g/2)^2)/myu-D; else 75 F2 = sqrt(Fs2^2+(ml*g/2)^2); end F1 = F2+D; else F1 = 0; F2 = 0; Fs1 = 0; Fs2 = 0; end F1_F2 =[F1;F2]; Fs1_Fs2 =[Fs1;Fs2]; Fx1_Fy1_Fx2_Fy2 = [-F1*c5-Fs1*s5;-F1*s5+Fs1*c5;F2*c5Fs2*s5;F2*s5+Fs2*c5]; % - Bộ điều khiển trượt bản: % function udk = fcn(qref,dqref,d2qref,q,dq,F,w) global m k b I L; udk = zeros(4,1); A1 = m(1)*k(1)^2+m(2)*L(1)^2+I(1); A2 = m(2)*k(2)^2+I(2); A3 = m(2)*L(1)*k(2); A4 = m(3)*k(3)^2+m(4)*L(3)^2+I(3); A5 = m(4)*k(4)^2+I(4); A6 = m(4)*L(3)*k(4); M = [A1+A2+2*A3*cos(q(2)), A2+A3*cos(q(2)), 0, 0; A2+A3*cos(q(2)), A2, 0, 0; 0, 0, A4+A5+2*A6*cos(q(4)), A5+A6*cos(q(4)) ; 76 0, 0, A5+A6*cos(q(4)), A5]; C = [ -A3*sin(q(2))*(dq(2)^2+dq(1)*dq(2))+b(1)*dq(1); A3*dq(1)^2*sin(q(2))+b(2)*dq(2); -A6*sin(q(4))*(dq(4)^2+dq(3)*dq(4))+b(3)*dq(3); A6*dq(3)^2*sin(q(4))+b(4)*dq(4)]; j11 = -L(1)*sin(q(1))-L(2)*sin(q(1)+q(2)); j12 = -L(1)*cos(q(1))-L(2)*cos(q(1)+q(2)); j21 = -L(2)*sin(q(1)+q(2)); j22 = -L(2)*cos(q(1)+q(2)); j33 = L(3)*sin(q(3))+L(4)*sin(q(3)+q(4)); j34 = -L(3)*cos(q(3))-L(4)*cos(q(3)+q(4)); j43 = L(4)*sin(q(3)+q(4)); j44 = -L(4)*cos(q(3)+q(4)); J = [j11 j12 0; j21 j22 0; 0 j33 j34; 0 j43 j44]; e = qref - q; de = dqref - dq; lamda = 10*eye(4); G = [lamda eye(4)]; S = G * [e;de]; B = [zeros(4);inv(M)]; fx = [dq ; inv(M)*(J*F-C-w)]; hesoK = [5;5;5;5]; udk = inv(G*B)*(G*[dqref;d2qref]-G*fx+lamda*S)+inv(G*B) 77 *diag([hesoK(1) hesoK(2) hesoK(3) hesoK(4)])*sign(S); % Bộ điều khiển trượt siêu xoắn: % function [udk,da]= fcn(qref,dqref,d2qref,q,dq,F,a,w) global m k b I L; udk = zeros(4,1); A1 = m(1)*k(1)^2+m(2)*L(1)^2+I(1); A2 = m(2)*k(2)^2+I(2); A3 = m(2)*L(1)*k(2); A4 = m(3)*k(3)^2+m(4)*L(3)^2+I(3); A5 = m(4)*k(4)^2+I(4); A6 = m(4)*L(3)*k(4); M = [A1+A2+2*A3*cos(q(2)), A2+A3*cos(q(2)), 0, 0; A2+A3*cos(q(2)), A2, 0, 0; 0, 0, A4+A5+2*A6*cos(q(4)), A5+A6*cos(q(4)) ; 0, 0, A5+A6*cos(q(4)), A5]; C = [ -A3*sin(q(2))*(dq(2)^2+dq(1)*dq(2))+b(1)*dq(1); A3*dq(1)^2*sin(q(2))+b(2)*dq(2); -A6*sin(q(4))*(dq(4)^2+dq(3)*dq(4))+b(3)*dq(3); A6*dq(3)^2*sin(q(4))+b(4)*dq(4)]; j11 = -L(1)*sin(q(1))-L(2)*sin(q(1)+q(2)); j12 = -L(1)*cos(q(1))-L(2)*cos(q(1)+q(2)); j21 = -L(2)*sin(q(1)+q(2)); j22 = -L(2)*cos(q(1)+q(2)); j33 = L(3)*sin(q(3))+L(4)*sin(q(3)+q(4)); 78 j34 = -L(3)*cos(q(3))-L(4)*cos(q(3)+q(4)); j43 = L(4)*sin(q(3)+q(4)); j44 = -L(4)*cos(q(3)+q(4)); J = [j11 j12 0; j21 j22 0; 0 j33 j34; 0 j43 j44]; e = qref - q; de = dqref - dq; lamda = 20*eye(4); G = [lamda eye(4)]; S = G * [e;de]; Beta =[5;5;5;5]; hesoK =[5;5;5;5]; B = [zeros(4);inv(M)]; fx = [ dq ; inv(M)*(J*F-C-w)] ; ueq = inv(G*B)*(G*[dqref;d2qref]-G*fx+lamda*S); usw = diag([beta(1) beta(2) beta(3)beta(4)]) *sqrt(abs(S)).*sign(S)+a; %Khoang bi chan [-U,U] U = 0.015; if abs(udk)>U da = udk; else da = -diag([hesoK(1) hesoK(2) hesoK(3) hesoK(4)])*sign(S); end udk = ueq+usw; % - 79 Bộ điều khiển trượt đầu cuối nhanh: % function udk = fcn(qref,dqref,d2qref,q,dq,F,w) global m k b I L; udk = zeros(4,1); A1 = m(1)*k(1)^2+m(2)*L(1)^2+I(1); A2 = m(2)*k(2)^2+I(2); A3 = m(2)*L(1)*k(2); A4 = m(3)*k(3)^2+m(4)*L(3)^2+I(3); A5 = m(4)*k(4)^2+I(4); A6 = m(4)*L(3)*k(4); M = [A1+A2+2*A3*cos(q(2)), A2+A3*cos(q(2)), 0, 0; A2+A3*cos(q(2)), A2, 0, 0; 0, 0, A4+A5+2*A6*cos(q(4)), A5+A6*cos(q(4)) ; 0, 0, A5+A6*cos(q(4)), A5]; C = [ -A3*sin(q(2))*(dq(2)^2+dq(1)*dq(2))+b(1)*dq(1); A3*dq(1)^2*sin(q(2))+b(2)*dq(2); -A6*sin(q(4))*(dq(4)^2+dq(3)*dq(4))+b(3)*dq(3); A6*dq(3)^2*sin(q(4))+b(4)*dq(4)]; j11 = -L(1)*sin(q(1))-L(2)*sin(q(1)+q(2)); j12 = -L(1)*cos(q(1))-L(2)*cos(q(1)+q(2)); j21 = -L(2)*sin(q(1)+q(2)); j22 = -L(2)*cos(q(1)+q(2)); j33 = L(3)*sin(q(3))+L(4)*sin(q(3)+q(4)); j34 = -L(3)*cos(q(3))-L(4)*cos(q(3)+q(4)); j43 = L(4)*sin(q(3)+q(4)); j44 = -L(4)*cos(q(3)+q(4)); J = [j11 j12 0; 80 j21 j22 0; 0 j33 j34; 0 j43 j44]; e = q - qref; de = dq - dqref; fx = inv(M)*(J*F-C-w); gx = inv(M); lg=0.3; xite = 0.1; alpha = 2.0; beta = 1.0; p0 = 9; q0 = 5; p = 3.0; q = 1.0; phi = 0.1; T1 = abs(e).^(q0/p0).*sign(e); s = de+alpha*e+beta*T1; % ham khuech dai bao hoa delta = 0.015; kk = 1/delta; if abs(s)>delta sats = sign(s); else sats = kk*s; end gamma = lg./abs(abs(s).^(q/p).*sats)+xite; udk = -inv(gx)*(fx-d2qref +alpha*de+beta*(q0/p0)*abs(e).^(q0/p0).*sign(e).*de +phi*s+gamma.*abs(s).^(q/p).*sats); 81 % Sơ đồ mô PIL cho điều khiển: 82 ... phần cứng dẫn đến gây ổn định trình điều khiển Chính thế, tác giả lựa chọn đề tài ? ?Điều khiển phi tuyến hệ thống tay máy đơi” đưa thuật tốn điều khiển phi tuyến bền vững nhằm khắc phục vấn đề... chung hệ thống tay máy đôi 1.2 Ứng dụng hệ thống tay máy đôi xu hướng nghiên cứu 1.3 Các nội dung nghiên cứu luận văn CHƯƠNG MƠ HÌNH HĨA ĐỐI TƯỢNG 2.1 Khảo sát hệ tay máy đôi. ..TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LUẬN VĂN THẠC SĨ Điều khiển phi tuyến hệ thống tay máy đôi VŨ HỮU TỈNH Ngành: Kỹ thuật điều khiển tự động hóa Giảng viên hướng dẫn: TS Nguyễn Tùng Lâm