Thiết kế bộ điều khiển và bộ giám sát lỗi cho mô hình cánh tay robot 4 bậc tự do

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH CƠNG TRÌNH NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN THIẾT KẾ BỘ ĐIỀU KHIỂN VÀ BỘ GIÁM SÁT LỖI CHO MƠ HÌNH CÁNH TAY ROBOT BẬC TỰ DO MÃ SỐ: SV2021-41 CHỦ NHIỆM ĐỀ TÀI: NGUYỄN THIỆN QUANG SKC 0 0 Tp Hồ Chí Minh, tháng 10/2021 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM BÁO CÁO TỔNG KẾT ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN THIẾT KẾ BỘ ĐIỀU KHIỂN VÀ BỘ GIÁM SÁT LỖI CHO MƠ HÌNH CÁNH TAY ROBOT BẬC TỰ DO SV2021-41 Thuộc nhóm ngành khoa học: Kỹ thuật SV thực : Nguyễn Thiện Quang Nam, Nữ: Nam Dân tộc: Kinh Năm thứ: /Số năm đào tạo: Lớp, khoa: 171511 Ngành học: Cơng nghệ kỹ thuật Điều khiển Tự động hóa Người hướng dẫn: TS Trần Đức Thiện TP Hồ Chí Minh, 10/2021 ix MỤC LỤC MỤC LỤC x DANH MỤC HÌNH ẢNH xiv DANH MỤC BẢNG xviii DANH MỤC NHỮNG TỪ VIẾT TẮT xix MỞ ĐẦU ❖ Tình hình nghiên cứu lý chọn đề tài ❖ Mục tiêu đề tài ❖ Đối tượng phạm vi nghiên cứu đề tài ❖ Phương pháp nghiên cứu ❖ Giới thiệu nội dung CHƯƠNG 1: CƠ SỞ LÍ THUYẾT 1.1 Định nghĩa robot: 1.2 Các loại cánh tay robot ứng dụng: 1.2.1 Robot giàn: 1.2.2 Robot hình trụ: 1.2.3 Robot hình cầu: 1.2.4 SCARA robot: 1.2.5 Robot song song: 1.3 Phân loại robot: 1.3.1 Phân loại theo kết cấu: 1.3.2 Phân loại theo điều khiển: x 1.3.3 Phân loại theo ứng dụng: 10 1.4 Động học robot: 10 1.4.1 Động học thuận cánh tay robot: 11 1.4.2 Động học nghịch cánh tay robot: 13 1.5 Quy hoạch quỹ đạo: 13 1.6 Động lực học cánh tay robot: 15 1.7 Các phương pháp truyền động cánh tay robot 20 1.8 Lý thuyết điều khiển PID 22 1.9 Lý thuyết điều khiển computed torque 24 1.10 Lý thuyết điều khiển Sliding mode: 25 1.11 Lý thuyết giám sát lỗi 26 CHƯƠNG 2: THIẾT KẾ VÀ THI CÔNG PHẦN CỨNG 27 2.1 Thiết kế phần khí: 27 2.1.1 Yêu cầu phần cứng: 27 2.1.2 Thiết kế mơ hình tối ưu mơ hình: 27 2.1.3 Lựa chọn thiết bị cho phần khí 30 2.2 Thiết kế phần điện: 33 2.3 Thi công phần cứng: 36 2.3.1 Thi công phần khí: 36 2.3.2 Thi công phần điện 37 CHƯƠNG 3: THUẬT TOÁN VÀ PHƯƠNG PHÁP ĐIỀU KHIỂN 41 3.1 Sơ đồ khối hệ thống 41 3.2 Động học cánh tay robot 41 3.2.1 Động học thuận 41 xi 3.2.2 Động học nghịch 45 3.3 Không gian làm việc quy hoạch quỹ đạo cho robot 47 3.3.1 Không gian làm việc 47 3.3.2 Quy hoạch quỹ đạo cho robot 48 3.4 Động lực học 49 3.5 Thiết kế chương trình mơ Matlab-Simulink 51 3.5.1 Chuyển đổi solidworks sang Simulink 51 3.5.2 Thành lập phương trình vi phần Matlab Function 52 3.6 Mô chưa có điều khiển 53 3.7 Thiết kế chương trình mơ có điều khiển 53 3.8 Thiết kế giám sát lỗi 56 3.9 Thiết kế điều khiển thực tế 57 3.9.1 Sơ đồ tổng quát hệ thống điều khiển 57 3.9.2 Lưu đồ giải thuật chương trình điều khiển 57 CHƯƠNG 4: KẾT QUẢ MÔ PHỎNG 59 4.1 Kết mơ chưa có điều khiển 59 4.1.1 Kết kiểm tra Simscape 59 4.1.2 Kết kiểm tra động học, quy hoạch quỹ đạo 60 4.2 Kết mơ có điều khiển 65 4.3 Kết giám sát lỗi 73 CHƯƠNG 5: KẾT QUẢ THỰC NGHIỆM 78 5.1 Kết phần cứng 78 5.2 Kết thực nghiệm 80 CHƯƠNG 6: KẾT LUẬN VÀ KIẾN NGHỊ 89 xii 6.1 Kết luận 89 6.2 Kiến nghị 89 TÀI LIỆU THAM KHẢO 90 PHỤ LỤC 92 BÀI BÁO KHOA HỌC 98 xiii DANH MỤC HÌNH ẢNH Hình 1 Cấu trúc robot giàn Hình Cấu trúc robot hình trụ Hình Cấu trúc robot hình cầu Hình Cấu trúc robot SCARA Hình Cấu trúc robot song song Hình Cấu trúc điều khiển vòng hở Hình Cấu trúc điều khiển vịng kín Hình Robot lắp ráp ô tô 10 Hình Máy xúc 10 Hình 10 Phân tích cấu trúc robot 10 Hình 11 Robot dị mìn 10 Hình 12 Cấu trúc động học thuận 11 Hình 13 Mơ hình hóa khâu khớp robot 11 Hình 14 Cấu trúc động học nghịch 13 Hình 15 Quy hoạch quỹ đạo robot 14 Hình 16 Cấu trúc điều khiển PID 22 Hình 17 Ảnh hưởng Kp lên hệ thống 23 Hình 18 Ảnh hưởng Ki lên hệ thống 23 Hình 19 Ảnh hưởng Kd lên hệ thống 24 Hình 20 Cấu trúc điều khiển tuyến tính hóa 25 Hình 21 Cấu trúc điều khiển Sliding mode 25 Hình 22 Cấu trúc điều khiển kiểm sốt lỗi …………………………………………26 Hình Khung robot bậc tự 27 Hình 2 Robot bậc tự phiên 28 Hình Robot bậc tự phiên 28 Hình Robot bậc tự phiên 28 Hình Robot bậc tự phiên 29 xiv Hình Sơ đồ khối phần điện 33 Hình Sơ đồ mạch điện chi tiết 37 Hình Sơ đồ nối dây nút nhấn 38 Hình Sơ đồ nối dây đèn báo 38 Hình 10 Sơ đồ nguyên lí mạch điều khiển Proteus 38 Hình 11 Bản vẽ PCB mạch điều khiển 39 Hình 12 Hình ảnh 3D mạch điều khiển 39 Hình Mơ hình cánh tay robot bậc tự 41 Hình Cấu trúc mơ hình cánh tay robot bậc tự 42 Hình 3 (a) Không gian làm việc Oxyz, (b) Không gian làm việc Oxy, (c) Không gian làm việc Oyz, (d) Không gian làm việc Oxz 47 Hình Chương trình mơ quỹ đạo 48 Hình (a) Quỹ đạo điểm, (b) Quỹ đạo điểm (c) Quỹ đạo điểm, (d) Quỹ đạo tròn 48 Hình Chức Mass Properties Solidworks 49 Hình Chương trình chuyển từ Solidworks sang Matlab-Simulink 51 Hình Bên ngồi khối phương trình vi phân cánh tay robot bậc tự 52 Hình Bên khối phương trình vi phân cánh tay robot bậc tự 52 Hình 10 Sơ đồ giải thuật chưa có điều khiển 53 Hình 11 Sơ đồ giải thuật có điều khiển 53 Hình 12 Cấu trúc điều khiển PD 54 Hình 13 Cấu trúc điều khiển PD-CTC 54 Hình 14 Cấu trúc điều khiển SMC 55 Hình 15 Cấu trúc giám sát lỗi 56 Hình 16 Lưu đồ điều khiển robot nút nhấn ON, PAUSE, HOME 58 Hình 17 Lưu đồ điều khiển vị trí động 59 Hình Quỹ đạo trịn tâm O(0.28;0;0.4)m bán kính R=0.14m 59 xv Hình (a) Giá trị bốn khớp quay [0;0;0;0] độ, (b) Giá trị bốn khớp quay [-90;90;-90;90] độ, (c) Giá trị bốn khớp quay [-90;-45;120;30] độ, (d) Giá trị bốn khớp quay [0;-45;60;90] độ 60 Hình Cấu trúc chương trình mơ chưa có điều khiển 60 Hình 4 Quỹ đạo trịn chưa có điều khiển 61 Hình Vị trí ngõ chưa có điều khiển 61 Hình (a) đáp ứng theta1, (b) đáp ứng theta2, (c) đáp ứng theta3, (d) đáp ứng theta4 chưa có điều khiển 63 Hình (a) Sai số theta1, (b) Sai số theta2, (c) Sai số theta3, (d) Sai số theta4 chưa có điều khiển 65 Hình Chương trình mơ PD, (b) Chương trình mơ CTC, (c) Chương trình mơ SMC 66 Hình Quỹ đạo trịn có điều khiển PD, CTC, SMC 67 Hình 10 Vị trí ngõ có điều khiển 67 Hình 11(a) đáp ứng khớp 1, (b) đáp ứng khớp 2, (c) đáp ứng khớp 3, (d) đáp ứng khớp điều khiển PD, CTC, SMC 69 Hình 12 (a) Sai số khớp 1, (b) Sai số khớp 2, (c) Sai số khớp 3, (d) Sai số khớp PD, CTC, SMC 70 Hình 13 (a) Tín hiệu điều khiển khớp 1, (b) Tín hiệu điều khiển khớp 2, (c) Tín hiệu điều khiển khớp 3, (d) Tín hiệu điều khiển khớp ba điều khiển PD, CTC, SMC 72 Hình 14 Cấu trúc quan sát lỗi, Fault Estimator (FE) 73 Hình 15 (a) Uớc lượng nhiễu khớp 1, (b) Ước lượng nhiễu khớp 2, (c) Ước lượng nhiễu khớp 4, (d) Ước lượng nhiễu khớp quan sát lỗi 75 Hình 16 (a) Sai số ước lượng nhiễu theta1, (b) Sai số ước lượng nhiễu theta2, (c) Sai số ước lượng nhiễu theta3, (d) Sai số ước lượng nhiễu theta4 quan sát lỗi 76 Hình Mơ hình thực tế cánh tay robot bậc tự 78 Hình Phần khí robot bậc tự 79 Hình (a) Nút nhấn, (b) Mạch điều khiển, (c) Tủ điều khiển 80 xvi Hình Chương trình thực nghiệm PD, (b) Chương trình thực nghiệm CTC, (c) Chương trình thực nghiệm SMC 81 Hình 5 (a) đáp ứng khớp 1, (b) đáp ứng khớp 2, (c) đáp ứng khớp 3, (d) đáp ứng khớp điều khiển PD, CTC, SMC 83 Hình (a) Sai số theta1, (b) Sai số theta2, (c) Sai số theta3, (d) Sai số theta4 điều khiển PD, CTC SMC 85 Hình (a) Tín hiệu điều khiển theta1, (b) Tín hiệu điều khiển theta2, (c) Tín hiệu điều khiển theta3, (d) Tín hiệu điều khiển theta4 có điều khiển CTC, SMC 87 xvii [11] L N Truc, N Van Quyen, and N P Quang (2020) Dynamic Model With a New Formulation of Coriolis/Centrifugal Matrix for Robot Manipulators J Comput Sci Cybern Vol 36, no 1, pp 89–104, doi: 10.15625/1813-9663/36/1/14557 [12] Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng (2005) Lý thuyết điều khiển tự động, Nhà xuất Đại học Quốc gia TP Hồ Chí Minh, Hồ Chí Minh [13] Antonio Visioli (2006) Practical PID Control, Springer, London [14] L R Rabiner and B Gold (1985) Analysis of Robot Dynamics and Compensation Using Classical and Computed Torque Techniques IEEE Transactions on Education Vol 6, no 0, pp 1308–1310, DOI: 10.1109/13.241614 [15] V Santibañez and R Kelly(2001) PD control with feedforward compensation for robot manipulators: Analysis and experimentation Robotica Vol 19, no 1, pp 11–19 [16] S Islam and X P Liu (2011) Robust sliding mode control for robot manipulators IEEE Trans Ind Electron Vol 58, no 6, pp 2444–2453, doi: 10.1109/TIE.2010.2062472 [17] S Han, M S Tran, and D Tran (2021) Adaptive Sliding Mode Control for a Robotic Manipulator with Unknown Friction and Unknown Control Direction Applied Sciences, pp 3919 [18] Duc Thien Tran, Hoang Vu Dao, Truong Quang Dinh, Kyoung Kwan Ahn (2020) Output Feedback Control via Linear Extended State Observer for an Uncertain Manipulator with Output Constraints and Input Dead-Zone Electronics 2020, 9(9), 1355, doi.org/10.3390/electronics9091355 [19] Zhiliang Zhao and Bao-Zhu Guo (2012) On Convergence of Nonlinear Active Disturbance Rejection Control for SISO Nonlinear Systems Control Conference (CCC) DOI:10.1109/CCDC.2012.6244560 [20] Linlin Li (2015) Fault Detection and Fault-Tolerant Control for Nonlinear Systems, Springer, Duisburg 91 PHỤ LỤC Chương trình tính động học thuận: function [p_x,p_y,p_z] = FORWARDKINEMATIC(the1,the2,the3,the4) d1=0.085; % thong so vat ly cua robot d2=0.1872; d3=0.14; d4=0.06; a1=0.3405; c1=cos(the1); % dat c1 s1 cho gon s1=sin(the1); c2=cos(the2); s2=sin(the2); c3=cos(the3); s3=sin(the3); c4=cos(the4); s4=sin(the4); T01= tran T12= T23= T34= T45= [ c1 -s1 0; s1 c1 0; 0 a1; 0 chuyen vi [ c2 -s2 d1; 0 -1 0; s2 c2 0 ; 0 [ c3 -s3 d2; s3 c3 0; 0 0; 0 [ c4 -s4 d3; s4 c4 0; 0 0; 0 [ 0 d4; 0; 0 0; 0 1]; 1]; % ma 1]; 1]; 1]; TFK= (T01*T12*T23*T34*T45); % ma tran chuyen vi tu diem dau cuoi ve toa goc st=TFK(3,1); ct=TFK(3,2); p_x=TFK(1,4); p_y=TFK(2,4); p_z=TFK(3,4); phi=atan(st/ct); 92 Chương trình tính động học nghịch: function[the11,the2,the3,the4]=INVERSEKINEMATIC(p_x,p_y,p_ z,phi) d1=0.085;% thong so vat ly cua robot, don vi met d2=0.1872; d3=0.14; d4=0.06; a1=0.3405; O31=sin(phi); %thanh phan ma tran Rotation O32=cos(phi); %thanh phan ma tran Rotation the1= atan(p_y/p_x); ;% don vi rad A= p_x*cos(the1) + p_y*sin(the1)-d1-d4*O32; B= p_z - a1 - d4*O31; costhe3= (A^2+B^2-d3^2-d2^2)/ (2*d2*d3); costhe3=max(min(costhe3,1),-1); sinthe3=sqrt(1-costhe3^2); the3=atan2(sinthe3,costhe3); % the3= acos(costhe3); C= d2 + d3*cos(the3); D= d3*sin(the3); sinthe2= (B*C-A*D)/(C^2+D^2); costhe2= (A+ D*sinthe2)/C; %costhe2=max(min(costhe2,1),-1); the2=atan2(sinthe2,costhe2); the4= phi-the2-the3; the11=the1; %theta=[the1;the2;the3;the4]; Chương trình quy hoạch quỹ đạo hình trịn: function [px_d,py_d,pz_d,phi]= Trajectory_Planning(t) f=0.1;%hz pz_d=0.4+0.14*sin(2*pi*f*t);%Vi tri truc z py_d=0.14*cos(2*pi*f*t);%Vi tri truc y px_d=0.38;%Vi tri truc z phi=0;% huong diem cuoi robot 93 Chương trình quy hoạch quỹ đạo điểm – điểm: function[px,py,pz,phi]=TRAJACTORYPLANNING4diem(p_x,p_y,p_z ,t,tf,phi1) if(mod(t,4*tf)