Tính toán động lực học và thiết kế điều khiển cộng tác cho hệ thống SMMS

LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN MỤC LỤC DANH MỤC CÁC BẢNG DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ LỜI CAM ĐOAN LỜI NÓI ĐẦU CẤU TRÚC LUẬN VĂN 10 CHƯƠNG 11 TỔNG QUAN 11 1.1 Giới thiệu hệ thống teleoperation 11 1.2 Ứng dụng hệ Teleoperation 12 1.2.1 Ứng dụng phẫu thuật 13 1.2.2 Ứng dụng thám hiểm không gian 13 1.2.3 Ứng dụng môi trường nguy hiểm 13 1.2.4 Mobile Robot 13 1.2.5 Một vài ứng dụng khác hệ Tele – Robot 14 1.3 Mục tiêu đóng góp luận văn 14 CHƯƠNG 15 CƠ SỞ LÝ THUYẾT 15 2.1 Lý thuyết ổn định Lyapunov 15 2.1.1 Định nghĩa 15 2.1.2 Tính ổn định Lyaponov 17 2.2 Đầu vào đến trạng thái ổn định 22 ĐỘNG LỰC HỌC HỆ TELEOPERATION 24 3.1 Giới thiệu 24 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN 3.1 Phương trình động lực học robot master hệ SMMS (mini robot) 29 3.2 Phương trình động lực học robot Slave (master hệ SMSS) 34 3.3 Độ trễ kênh truyền thông 37 3.4 Động lực học môi trường 38 CHƯƠNG 42 THIẾT KẾ ĐIỀU KHIỂN CHO HỆ SMSS 42 4.1 Mục tiêu điều khiển 42 4.2 Thiết kế luật điều khiển 42 4.3 Phân tích tính ổn định hệ thống 44 4.4 Kết mô 46 CHƯƠNG 56 THIẾT KẾ ĐIỀU KHIỂN CHO HỆ SMMS 56 5.1 Mục tiêu điều khiển 56 5.2 Phân tách thụ động (Passive-Decomposition) 57 5.3 Định hình trở kháng (Impedance Shaping) 59 5.4 Luật điều khiển cộng tác hệ thống SMMS 60 5.4.1 Luật điều khiển hệ thống Shape 60 5.4.2 Luật điều khiển hệ thống Lock 61 5.6 Kết mô 67 Chương 76 KẾT LUẬN 76 6.1 Kết luận 76 6.2 Hướng phát triển 76 PHỤ LỤC 78 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN CÁC KHỐI CỦA CHƯƠNG TRÌNH MÔ PHỎNG 78 CÁC FILE CODE TRONG CHƯƠNG TRÌNH MÔ PHỎNG 84 TÀI LIỆU THAM KHẢO 90 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN DANH MỤC CÁC BẢNG Bảng 4.1 Các thông số Robot…………………………………………………… 47 Bảng 4.2 Các thông số mô hệ SMSS………………………………………… 47 Bảng 4.3 Giá trị trễ kênh truyền thông……………………………………… 48 Bảng 5.1 Bảng tham số mini robot bậc tự do………………………………… 68 Bảng 5.2 Bảng tham số DD robot bậc tự do………………………………… 68 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ CHƯƠNG 1: TỔNG QUAN HỆ THỐNG TELEOPERATION Hình 1.1 Hệ Teleoperation song phương…………………………………………… 12 CHƯƠNG 2: CƠ SỞ LÝ THUYẾT Hình 2.1 Vùng xung quanh gốc tọa độ bán kính r…………………………… 16 CHƯƠNG 3: ĐỘNG LỰC HỌC HỆ TELEOPERATION Hình 3.1 Hệ thống điều khiển từ xa gồm robot master robot slave 24 Hình 3.2 Hệ thống teleoperation robot master nhiều robot slave……………… 27 Hình 3.3 Robot bậc tự dạng tay liên kết song song…………………………… 29 Hình 3.4 Robot bậc tự dạng tay nối tiếp……………………………………… 34 Hình 3.5 Mô hình thời gian trễ……………………………………………………… 38 Hình 3.6 Mô hình môi trường……………………………………………………… 39 CHƯƠNG : THIẾT KẾ ĐIỀU KHIỂN CHO HỆ SMSS Hình 4.1 Sơ đồ khối điều khiển hệ SMSS…………………………………………… 44 Hình 4.2 Robot bậc tự dạng tay nối tiếp……………………………………… 46 Hình 4.3 Vị trí môi trường với Robot Master Slave …………………………… 47 Hình 4.4 Vị trí Master Slave có trễ………………………………………… 49 Hình 4.5 Vận tốc Master Slave có trễ……………………………………… 49 Hình 4.6 Lực tác động người môi trường…………………………………… 50 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Hình 4.7 Vị trí Master Slave có trễ……………………………………… 50 Hình 4.8 Vận tốc Master Slave có trễ…………………………………… 51 Hình 4.9 Lực tác động người môi trường…………………………………… 51 Hình 4.10 Vị trí Master Slave có trễ………………………………………… 52 Hình 4.11 Vận tốc Master Slave có trễ……………………………………… 52 Hình 4.12 Lực tác động người môi trường……………………………… 53 Hình 4.13 Vị trí Master Slave có trễ………………………………………… 53 Hình 4.14 Vận tốc Master Slave có trễ……………………………………… 54 Hình 4.15 Lực tác động người môi trường………………………………… 54 CHƯƠNG 5: THIẾT KẾ ĐIỀU KHIỂN CHO HỆ SMMS Hình 5.1 Hệ thống teleoperation SMMS…………………………………………… 56 Hình 5.2 Cấu trúc hệ thống SMMS………………………………………………… 52 Hình 5.3 Sơ đồ khối điều khiển hệ thống SMMS…………………………………… 62 Hình 5.4 Vị trí hệ thống Shape xSd …………………………………………… 71 Hình 5.5 Vị trí master hệ thống Lock…………………………………………… 71 Hình 5.6 Lực tay máy ti lệ hệ thống Lock…………………………………… 72 Hình 5.7 Lực gắp hệ thống Shape…………………………………………… 72 Hình 5.8 Vị trí hệ thống Shape xSd …………………………………………… 73 Hình 5.9 Vị trí master hệ thống Lock…………………………………………… 73 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Hình 5.10 Lực tay máy ti lệ hệ thống Lock………………………………… 74 Hình 5.11 Lực gắp hệ thống Shape…………………………………………… 74 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN LỜI CAM ĐOAN Tên Nguyên Xuân Thuận, học viên cao học khóa 2012B.CĐT.KH chuyên ngành Cơ Điện Tử Sau gần năm học tập, nghiên cứu trường Đại học Bách Khoa Hà Nội Đươc hướng dẫn thầy cô giáo, đặc biệt TS Đỗ Đức Nam, hoàn thành xong luận văn tốt nghiệp thạc sĩ Với đề tài luận văn tốt nghiệp là: “Tính toán động lực học tính toán điều khiển cộng tác cho hệ thống SMMS-Teleoperation với độ trễ kênh truyền thông” Tôi xin cam đoan công trình nghiên cứu cá nhân hướng dẫn TS Đỗ Đức Nam tham khảo tài liệu liệt kê Tôi không chép công trình cá nhân khác hình thức Nếu có xin hoàn toàn chịu trách nhiệm Hà Nội, ngày 26 tháng 03 năm 2014 Người cam đoan Nguyễn Xuân Thuận LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN LỜI NÓI ĐẦU Hiện nay, với phát triển nhân loại, ngày nhiều công việc đòi hỏi trình độ cao kèm với áp lực, độ nguy hiểm lớn Để hoàn thành công việc trước khó khăn, nhiên từ hệ thống Robot song phương đưa vào sử dụng giúp người thực việc từ xa với độ xác cao vai trò Teleoperation ngày quan trọng Để nâng cao độ tin cậy hệ thống hiệu làm việc phương pháp điều khiển cần phải cải tiến nhiều Với yêu cầu trên, để bổ sung nâng cao kiến thức điều khiển phần nhằm làm sở cho trình nghiên cứu làm việc sau này, học viên chọn đề tài:“ Tính toán động lực học tính toán điều khiển cộng tác cho hệ thống SMMS-Teleoperation với độ trễ kênh truyền thông” Đây đề tài quan tâm lớn từ phía nhà khoa học khắp giới, tiêu biểu Nhật Châu Âu… Trong trình thực đề tài này, học viên đưa số kết khả quan Để kết đó, học viên xin chân thành cảm ơn giúp đỡ tận tình TS Đỗ Đức Nam thầy, cô giáo môn Cơ sở thiết kế máy Robot tạo điều kiện cho học viên hoàn thành đề tài cách tốt Tuy nhiên, với nhiều khó khăn vấn đề thời gian kinh nghiệm nên luận văn không tránh khỏi sai sót Kính mong quý thầy cô đóng góp ý kiến để đề tài hoàn thành tốt Trường Đại học Bách Khoa Hà Nội HVTH Nguyễn Xuân Thuận LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN CẤU TRÚC LUẬN VĂN Chương 1: Tổng quan Giới thiệu chung Ở phần đầu chương giới thiệu hệ thống teleoperation Các thành phần hệ thống thông số yêu cầu đặt Phân tích tổng quát vài nghiên cứu trước Phần chương ứng dụng teleoperation, mục tiêu đóng góp đồ án Chương 2: Cơ sở lý thuyết Đưa số kiến thức cần thiết kết đồ án dựa trên: lý thuyết ổn định Lyapunov với định nghĩa định lý bản; ổn định hệ thống có nhiễu, lý thuyết ứng dụng cho hệ thống động lực học môi trường phân tích tính ổn định hệ thống Chương 3: Động lực học hệ thống teleoperation Đề cập đến động lực học hệ thống teleoperation bao gồm Robot Master Robot Slave Chương giới thiệu hệ thống SMSS(single master single slave) hệ thống SMMS(single master multi slave) Chương 4: Thiết kế điều khiển hệ thống SMSS theo phương pháp Scattering Đưa thông số điều khiển Scattering cho hệteleoperation,kết hợp với Virtual Damping ứng dụng cho hệ thống SMSS để cải tiến bám sát thực thi đồng Trong phân tích tính ổn định phần này, sử dụng lý thuyết Lyapunov để chứng minh hệ “ ổn định tiệm cận” Chương 5: Thiết kế luật điều khiển cho hệ thống SMMS Một phương pháp đề xuất cho hệ thống SMMS Trong chương tính ổn định phân tích theo lý thuyết Lyapunov số kết mô cụ thể đưa hình vẽ Chương 6: Kết luận Đưa vài kết luận từ phương pháp nêu có số đề xuất tương lai 10 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Một hướng nghiên cứu tiếp đề tài nghiên cứu hệ thống hỗ trợ cho người, cụ thể hệ walking model Hệ thống giúp người điều khiển sử dụng robot master cách hiệu dễ dàng 77 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN PHỤ LỤC CÁC KHỐI CỦA CHƯƠNG TRÌNH MÔ PHỎNG A Hệ thống SMSS dùng Scattering & Virtual Damping Hình A.1 Khối điều khiển hệ thống SMSS Hình A.2 Khối điều khiển Master 78 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Hình A.3 Khối Master Hình A.4 Khối điều khiển Slave 79 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Hình A.5 Khối Slave Hình A.6 Khối Human 80 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Hình A.7 Khối môi trường B Hệ thống SMMS dùng luật điều khiển đề xuất Hình B.1 Khối mô hệ thống SMMS 81 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Hình B.2 Khối môi trường Hình B.3 Khối mini robot Hình B.4 Khối điều khiển hệ thống master 82 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Hình B.5 Khối điều khiển hệ thống Lock Hình B.6 Khối hệ thống Shape 83 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN CÁC FILE CODE TRONG CHƯƠNG TRÌNH MÔ PHỎNG C.1 File Paremeter cho hệ thống SMSS clear all %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% qs1=-45*(pi/180); qs2=90*(pi/180); qm1=-45*(pi/180); qm2=90*(pi/180); zm1=0.2828; zm2=0; zs1=0.2828; zs2=0.0; l1=0.2; l2=0.2; x=200; y=2; b=900; z=0; Kd=200; Kp=200; D=150; zm1=0.2828; zm2=0; zs1 = 0.2828; zs2 = 0; Tm=0.01; Ts=0.01; del_Tm=0.015; del_Ts=0.015; freq_m=0.3;%0.3; 84 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN freq_s=0.3;%.3; dTm=0.5; dTs=0.5; t=0.015; C.2 File Parameter cho hệ thống SMMS clear all; M1_1=0.376; M2_1=0.0326; M3_1=0.0372; theta1_1=(0*pi)/180; theta2_1=(90*pi)/180; B1_1=0; B2_1=0; D1_1=0; D2_1=0; M1_2=0.376; M2_2=0.0326; M3_2=0.0372; theta1_2=(0*pi)/180; theta2_2=-(90*pi)/180; B1_2=0; B2_2=0; D1_2=0; D2_2=0; mM1=0.00149; mM2=-0.000764; mM3=0.000686; mtheta1=(0*pi)/180; mtheta2=(90*pi)/180; mB1=0; 85 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN mB2=0; mD1=0; mD2=0; Tm=0.1;%0.2 Ts=0.05;%0.2 del_Tm=0.14; del_Ts=0.1; freq_m=1; freq_s=1; Kd_m= [25 ;0 25]; Kp_m= [150 ;0 150]; Ki=[0.005 ; 0.005]; Kd_L= [55 ;0 45]; Kp_L= [320 ;0 320]; Ki=[0.005 ; 0.005]; Kd_S= [30 ;0 30]; Kp_S= [200 ;0 205]; KF_m= [0.5 ;0 0.5]; KF_L= [0.5 ;0 0.5]; beta=[2 ;0 2]; beta1=[1/2 ;0 1/2]; D_XE= [0.52 ; 0]; D_XE1= [0.46 ; 0]; H=[1 0;0 1]; H1=inv(H); a=1/1.5625; S=[ -1 0 -1 a/2 a/2 a/2 0 a/2 ]; 86 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN S1=S'; S2=inv(S); S3=S2'; Clc C.3 File MiniRobot function [sys,x0,str,ts] = minirobo(t,x,u,flag,mM1,mM2,mM3,mtheta1,mtheta2) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes(mtheta1,mtheta2); case 1, sys=mdlDerivatives(t,x,u,mM1,mM2,mM3);; case 2, sys=mdlUpdate(t,x,u); case 3, sys=mdlOutputs(t,x,u); case 4, sys=mdlGetTimeOfNextVarHit(t,x,u); case 9, sys=mdlTerminate(t,x,u); otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes(mtheta1,mtheta2) sizes = simsizes; sizes.NumContStates = 4;; sizes.NumDiscStates = 0; sizes.NumOutputs = 2; sizes.NumInputs = 2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 = [mtheta1,mtheta2,0,0]; str = []; ts = [0 0]; function sys=mdlDerivatives(t,x,u,mM1,mM2,mM3) M=[mM1 mM2*cos(x(1)-x(2));mM2*cos(x(1)-x(2)) mM3]; C=[0 x(4)*mM2*sin(x(1)-x(2));-x(3)*mM2*sin(x(1)-x(2)) 0]; tmp=inv(M)*(u-C*[x(3);x(4)]); sys(1)=x(3); 87 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN sys(2)=x(4); sys(3)=tmp(1); sys(4)=tmp(2); function sys=mdlUpdate(t,x,u) sys = []; function sys=mdlOutputs(t,x,u) sys(1) = x(1); sys(2) = x(2); function sys=mdlGetTimeOfNextVarHit(t,x,u) sampleTime = 0; sys = []; function sys=mdlTerminate(t,x,u) sys = []; C.4 File DDrobot function [sys,x0,str,ts] = DDrobot(t,x,u,flag,M1,M2,R,B1,B2,D1,D2,theta1,theta2) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes(theta1,theta2); case 1, sys=mdlDerivatives(t,x,u,M1,M2,R,B1,B2,D1,D2); case 2, sys=mdlUpdate(t,x,u); case 3, sys=mdlOutputs(t,x,u); case 4, sys=mdlGetTimeOfNextVarHit(t,x,u); case 9, sys=mdlTerminate(t,x,u); otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes(theta1,theta2) sizes = simsizes; sizes.NumContStates = 4;%x1,x2,x3,x4 sizes.NumDiscStates = 0; sizes.NumOutputs = 4;%y1,y2,y3,y4: -> q1,q2,dq1,dq2; sizes.NumInputs = 2;% - is Tau1,Tau sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 = [theta1,theta2,0,0]; 88 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN str = []; ts = [0 0]; function sys=mdlDerivatives(t,x,u,M1,M2,R,B1,B2,D1,D2) M=[M1+2*R*cos(x(2)) M2+R*cos(x(2));M2+R*cos(x(2)) M2 C=[-2*R*x(3)*x(4)*sin(x(2))R*x(4)*x(4)*sin(x(2));R*x(3)*x(3)*sin(x(2))];%tmp=inv(M)*(u-C); sys(1)=x(3); % dq1 sys(2)=x(4); % dq2 sys(3)=tmp(1); % ddq1 sys(4)=tmp(2); % ddq2 function sys=mdlUpdate(t,x,u) sys = []; function sys=mdlOutputs(t,x,u) sys(1) = x(1);% q1 sys(2) = x(2);% q2 sys(3) = x(3);%dq1 sys(4) = x(4);%dq2 function sys=mdlGetTimeOfNextVarHit(t,x,u) sampleTime =1; sys = []; function sys=mdlTerminate(t,x,u) sys = []; 89 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN TÀI LIỆU THAM KHẢO Paul Berestesky, Nikhil Chopra, and Mark W.Spong, Discrete Time Passivity in Bilateral Teleoperation over the Internet N Hogan, Impedance control: An approach to manipulation, JSME Trans Journal of Dynamics System, Measurement, and Control, Vol 107, pp.1-24, 1985 H C Cho and J H Park, Impedance control with variable damping for bilateral teleoperation under time delay, JSME Inter Journal, serial C, Vol 48, No.4, pp 695-703, 2005 H Khalil, Nonlinear systems Prentice Hall, Upper Saddle River, NJ07458, 1996 J Funda, and R P Paul, A symbolic teleoperator interface for time-delayed underwater robot manipulation, In Ocean technologies and opportunities in the Pacific for the 90s, pp 1526-1533, 1991 R Uhrich, Terminus controlled deep ocean manipulator, OCEANS, 5, pp 301304, 1973 A J Madhani, G Niemeyer, and Jr J K Salisbury, The black falcon: A teleoperated surgical instrument for minimally invasive surgery, In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems, vol 2, pp 936-944, 1998 D Lee, O Martinez-Palafox, and M W Spong, Passive bilateral teleoperation of a wheeled mobile robot over a delayed communication network In Proceedings of the IEEE international conference on robotics and automation, pp 3298-3303, Orlando, FL, 2006 G.Tao, A Simple Alternative to the Barbalat Lemma IEEE Trans on Automatic Control, Vol 42, No 5, pp 698, 1994 10 DO DUC NAM, Cooperative Control of Teleoperation System with Fourchannel Force Reection, 2010 Division of Electrical Engineering and Computer Science 90 LUẬN VĂN THẠC SĨ NGUYỄN XUÂN THUẬN Graduate School of Natural Science & Technology Kanazawa University 11 D J Lee and M W Spong, Passive bilateral control of teleoperators under constanttime-delay, In Proceedings of the IFAC World Congress, 2005 12 Giáo trình robot công nghiệp – TS Phạm Đăng Phước 13 Nghiên cứu động lực học thiết kế điều khiển cho hệ thống Teleoperation, Nguyễn Xuân Bình, Nguyễn Trung Kiên, CĐT – K51 14 Lý thuyết điều khiển phi tuyến, Nguyễn Doãn Phước, NXB Khoa học 91 ... định hệ thống có nhiễu, lý thuyết ứng dụng cho hệ thống động lực học môi trường phân tích tính ổn định hệ thống Chương 3: Động lực học hệ thống teleoperation Đề cập đến động lực học hệ thống. .. điều khiển cộng tác hệ thống SMMS 60 5.4.1 Luật điều khiển hệ thống Shape 60 5.4.2 Luật điều khiển hệ thống Lock 61 5.6 Kết mô 67 Chương 76 KẾT... viên chọn đề tài:“ Tính toán động lực học tính toán điều khiển cộng tác cho hệ thống SMMS-Teleoperation với độ trễ kênh truyền thông” Đây đề tài quan tâm lớn từ phía nhà khoa học khắp giới, tiêu