Điều khiển vị trí và lực tương tác trong hệ thống SMSS Teleoperation Tổng quan về hệ thống teleoperation. Tính toán động học robot trong hệ SMSS teleoperation. Động lực học robot trog hệ thống SMSS teleoperation. Thiết kế điều khiển hệ SMSS teleoperation.
BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - NGUYỄN TUẤN ANH THIẾT KẾ ĐIỀU KHIỂN VỊ TRÍ VÀ LỰC TƯƠNG TÁC TRONG HỆ THỐNG SMSS TELEOPERATION LUẬN VĂN THẠC SĨ KỸ THUẬT CƠ ĐIỆN TỬ Hà Nội –2013 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI NGUYỄN TUẤN ANH THIẾT KẾ ĐIỀU KHIỂN VỊ TRÍ VÀ LỰC TƯƠNG TÁC TRONG HỆ THỐNG SMSS TELEOPERATION LUẬN VĂN THẠC SĨ KỸ THUẬT CHUYÊN NGÀNH:CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC TS ĐỖ ĐỨC NAM Hà Nội –2013 MỤC LỤC LỜI CAM ĐOAN DANH MỤC CÁC BẢNG DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ LỜI NÓI ĐẦU CẤU TRÚC LUẬN VĂN CHƯƠNG I: TỔNG QUAN HỆ THỐNG TELEOPERATION 1.1 Giới thiệu chung 1.2 Ứng dụng hệ Teleoperation 11 1.2.1 Ứng dụng phẫu thuật từ xa 12 1.2.2 Ứng dụng thám hiểm không gian 12 1.2.3 Ứng dụng môi trường nguy hiểm 14 1.2.4 Mobile Robot 14 1.2.5 Một vài ứng dụng đặc điểm khác hệ thống Teleoperation 15 1.3 Mục tiêu đóng góp đề tài 15 CHƯƠNG II :TÍNH TỐN ĐỘNG HỌC ROBOT TRONG HỆ SMSS TELEOPERATION 17 2.1 Các phép biểu diễn biến đổi tọa độ 17 2.1.1 Nội dung toán động học robot 17 2.1.2 Tọa độ ma trận biến đổi tọa độ 17 2.1.3 Ma trận Denavit-Hartenberg 22 2.2 Động học thuận robot 26 2.3 Động học ngược robot 28 CHƯƠNG III: ĐỘNG LỰC HỌC ROBOT TRONG HỆ THỐNG SMSS TELEOPERATION 30 3.1 Cơ sở toán học 30 3.1.1 Vân tốc điểm robot 30 3.1.2 Tính động vi khối lượng dm 32 3.1.3 Tính robot 34 3.1.4 Hàm Lagrange 34 3.1.5 Phương trình động lực học robot 35 3.2 Động lực học robot hệ SMSS 37 3.2.1 Phương trình động lực học robot Master 37 3.2.2 Phương trình động lực học robot Slave 43 3.3 Động lực học môi trường 49 3.4 Độ trễ kênh truyền thông 52 CHƯƠNG IV:THIẾT KẾ ĐIỀU KHIỂN HỆ SMSS TELEOPERATION 54 4.1 Đối tượng điều khiển 54 4.2 Thiết kế hệ thống điều khiển PD kết hợp điều khiển lực 54 4.3.Phân tích tính ổn định hệ thống 56 4.4 Kết mô 62 4.4.1.Kết mô khơng có thời gian trễ 65 4.4.2.Kết mô thời gian trễ 66 CHƯƠNG V :KẾT LUẬN 68 5.1 Kết luận 68 5.2 Hướng phát triển 68 TÀI LIỆU THAM KHẢO 69 PHỤ LỤC 71 LỜI CAM ĐOAN Tên Nguyễn Tuấn Anh, học viên cao học khóa 2011B.CĐT.KT 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, tơi hồn thành xong luận văn tốt nghiệp Với đề tài luận văn tốt nghiệp là: “Điều khiển vị trí lực tương tác hệ thống SMSS Teleoperatiton” Tôi xin cam đoan cơng trình nghiên cứu cá nhân tơi 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ó tơi xin hồn toàn chịu trách nhiệm Hà Nội, ngày tháng năm 2013 Người cam đoan Nguyễn Tuấn Anh DANH MỤC CÁC BẢNG Bảng 2.1 Bảng thông số DH……………………………………………………….…26 Bảng 4.1 Bảng tham số robot Mini………………………………………………… 62 Bảng 4.2 Bảng tham số robot Slave………………………………………………….63 Bảng 4.3 Bảng tham số điều khiển………………………………………………… 63 Bảng 4.4 Các trường hợp mô phỏng…………………………………………………64 DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ CHƯƠNG 1: TỔNG QUAN HỆ THỐNG TELEOPERATION Hình 1.1 Mơ hình hệ thống Teleoperation ……………………………………… 11 Hình 1.2 Mơ hình hệ thống Teleoperation y tế……………………………… 12 Hình 1.3 Hình ảnh hệ thống Teleoperation sử dụng khơng gian …………… 12 Hình 1.4 Hình ảnh hệ thống Teleoperation sử dụng môi trường nguy hiểm… 14 Hình 1.5 Mơ hình Mobile Robot…………………………………………………… 14 CHƯƠNG II: TÍNH TỐN ĐỘNG HỌC HỆ THỐNG SMSS TELEOPERATION Hình 2.1 Biểu diễn điểm khơng gian…………………………………… 18 Hình 2.2 Các hệ tọa độ quay……………………………………………………… 18 Hình 2.3 Hệ tọa độ biến đổi …………………………………………………………20 Hình 2.4 Robot n khâu…………………………………………………………… 23 Hình 2.5 Hai khâu liên tiếp……………………………………………………… …24 Hình 2.6 Robot hai khâu ……………………………………………………….……26 CHƯƠNG III: ĐỘNG LỰC HỌC HỆ THỐNG SMSS TELEOPERATION Hình 3.1 Khảo sát vi khối lượng dm tay robot………………………… …31 Hình 3.2 Mơmen qn trính độc cực…………………………………………… 33 Hình 3.3 Hệ thống điều khiển từ xa gồm robot Master robot Slave (SMSS)37 Hình 3.4 Robot Master mini - bậc tự ………………………………………… 37 Hình 3.5 Robot Slave - bậc tự ……………………………………………….…43 Hình 3.6 Mơ hình mơi trường tác động.………………………………………… …49 Hình 3.7 Mơ hình thời gian trễ…………………………………………………….…52 CHƯƠNG IV : THIẾT KẾ ĐIỀU KHIỂN HỆ THỐNG SMSS TELEOPERATION Hình 4.1 Sơ đồ khối điều khiển hệ thống SMSS………………………………….…56 Hình 4.2 Vị trí Robot Slave tương tác với mơi trường………………………… 63 Hình 4.3 Biểu đồ vị trí robot robot Slave chưa va chạm với môi trường khơng có trễ…… ………… ……………………………………………………65 Hình 4.4 Biểu đồ lực robot robot Slave chưa va chạm với mơi trường khơng có trễ ……………………………………………………………………….…65 Hình 4.5 Biểu đồ vị trí robot robot Slave va chạm với mơi trường khơng có trễ ……………………………………………………………………….…65 Hình 4.6 Biểu đồ lực robot robot Slave va chạm với mơi trường khơng có trễ ………………………………………………………………………… ….…65 Hình 4.7 Biểu đồ vị trí robot robot Slave chưa va chạm với môi trường với trễ kênh truyền thơng…………………………………………………… ….66 Hình 4.8 Biểu đồ lực robot robot Slave chưa va chạm với môi trường với trễ kênh truyền thơng ………………………………….…………………… …66 Hình 4.9 Biểu đồ vị trí robot robot Slave va chạm với môi trường với trễ kênh truyền thơng …………………………… ……………………………… 66 Hình 4.10 Biểu đồ lực robot robot Slave va chạm với môi trường với trễ kênh truyền thơng… …………………………… .…….66 LỜI NĨI ĐẦU Hiện nay, có nhiều cơng việc địi hỏi áp lực lớn, độ nguy hiểm cao mức bình thường nhiều người tiếp cận trực tiếp để thao tác Việc hồn thành cơng việc trước khó khăn, nhiều bất khả thi Tuy nhiên với khoa học công nghệ ngày phát triển, công việc giải hiệu quả, xác, an tồn Ví dụ hệ thống Robot song phương điều khiển từ xa (hay gọi hệ thống Teleoperation) đưa vào sử dụng giúp người thực cơng việc từ xa với độ xác cao, kết thu đáng kể Vì vai trò hệ thống ngày quan trọng cần phát triển Để nâng cao độ tin cậy, tính hiệu quả, ổn định hệ thống việc cải tiến kết cầu khí để đảm bảo độ cứng vững cấu trúc, việc cải tiến nâng cao phương pháp điều khiển quan trọng Sau tìm hiểu kiến thức chuyên sâu hệ thống Robot điều khiển từ xa, tác giả nhận thấy tầm quan trọng ý nghĩa thực tiễn hệ thống khả phát triển tương lai Do định chọn đề tài nghiên cứu :”Thiết kế điều khiển vị trí lực tương tác hệ thống SMSS Teleoperation” Vấn đề điều khiển vị trí lực có nhiều nhà nghiên cứu trước quan tâm, nhiên kết cịn cần phải cải thiện nhiều độ xác, tính ổn định tính hiệu phương pháp điều khiển Trong trình thực đề tài này, tác giả thu số kết khả quan thỏa mãn số mục tiêu đưa đề tài Để kết đó, tác giả xin chân thành cảm ơn giúp đỡ tận tình TS Đỗ Đức Nam hướng dẫn tận tình, tạo điều kiệ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 đồ án không tránh khỏi sai sót Kính mong q thầy đóng góp ý kiến để đề tài hồn thành tốt CẤU TRÚC LUẬN VĂN Luận văn xây dựng gồm chương sau: Chương : Tổng quan Ở phần đầu chương giới thiệu hệ thống Teleoperation, thành phần hệ thống thơng số u cầu đặt Phân tích tổng quát vài nghiên cứu trước Phần chương mở đầu ứng dụng hệ thống Teleoperation, sau mục tiêu đóng góp luận văn Chương : Tính tốn động học Robot hệ thống SMSS Teleoperation Chương đề cập tính tốn, biến đổi, tốn động học thuận, toán động học ngược Robot Scara bậc tự làm sở cho việc giải tốn vị trí Robot Chương : Động lực học Robot hệ thống SMSS Teleoperation Hệ thống Teleoperation sử dụng gồm Robot tương tác với nhau, tiến hành xây dựng phương trình động lực học cho Robot (Robot Master Robot Slave có cấu trúc khác nhau) để làm tảng cho việc đề xuất phương pháp điều khiển hệ thống Chương : Thiết kế điều khiển hệ thống SMSS Teleoperation Trên sở phương trình động lực học xây dựng chương 3, chương tác giả sử dụng phương pháp phản hồi thụ động để làm sở đề xuất phương pháp điều khiển cho phía hệ thống Sử dụng kỹ thuật liên quan đến phương pháp Lyapunov - từ đầu vào ổn định đến trạng thái ổn định (ISS) để chứng minh tính ổn định phương pháp điều khiển đưa Một vài mô kết thực nghiệm để tính hiệu phương pháp đề xuất Chương : Kết luận Đưa kết luận kết phương pháp điều khiển đề xuất, đánh giá tính xác, hiệu đưa giải pháp tương lai 4.4.2.Kết mô với thời gian trễ kênh truyền thông zm zs 0.28 0.26 0.24 0.22 0.2 zm zs 0.2 Truc y (m) Truc x (m) Khi Robot Slave chuyển động tự : 0.1 -0.1 10 20 30 40 50 10 20 30 40 50 thoi gian (s) thoi gian (s) Hình 4.7 Biểu đồ vị trí robot robot Slave chuyển động tự Hình 4.8 Biểu đồ lực robot robot Slave chuyển động tự Khi Robot Slave va chạm với môi trường : zm zs 0.26 0.25 0.1 Truc y (m) Truc x (m) 0.27 zm zs 0.05 -0.05 0.24 va cham 10 20 30 40 50 thoi gian (s) 10 20 30 40 50 thoi gian (s) Hình 4.9 Biểu đồ vị trí robot robot Slave va chạm với mơi trường Hình 4.10 Biểu đồ lực robot robot Slave va chạm với môi trường 66 Nhận xét: Kết mơ Hình 4.3 – 4.6 trường hợp mơ Robot Slave chuyển động tự theo Robot Master khơng có trễ kênh truyền thơng Ta thấy quỹ đạo vận tốc hai Robot bám sát với sai số nhỏ Hình 4.3 biểu diễn lực tác động người điều khiển lên Robot Master lực phản hồi Robot Slave, lúc chuyển động tự lực phản hồi Robot Slave hồn tồn khơng, nhiên khâu làm việc có tương tác với mơi trường lực có dạng giống với lực điều khiển phía Master thời gian va chạm, va chạm vị trí hai Robot đảm bảo đồng Từ hình 4.7 đến 4.10 biểu đồ ứng với trường hợp robot Slave va chạm với mơi trường kênh truyền thơng có trễ bé Ta nhận thấy robot Master Slave đảm bảo đồng yếu tố lực vị trí Tuy nhiên bắt đầu xuất nhiễu lớn Hai Robot có khác hình dạng cấu trúc kết điều khiển cho thấy vị trí lực đảm bảo trùng khớp theo tỉ lệ phương pháp điều khiển có xuất lực vị trí đầu vào Với phương pháp điều khiển đề xuất cho hệ SMSS, kết đạt thỏa mãn mục tiêu nhiệm vụ điều khiển Phương pháp có số ưu điểm sau : + Trong luật điều khiển có xuất momen đầu vào, nhờ việc điều khiển lực trở nên xác + Thêm điều kiện cản môi trường tính tốn lực phản hồi làm cho việc điều khiển vị trí trở lên xác 67 CHƯƠNG V :KẾT LUẬN 5.1 Kết luận Bản luận văn đề xuất phương pháp điều khiển đồng cho hệ thống Teleoperation với cấu hình hai Robot khác thời gian trễ kênh truyền thông biến thiên Robot Master Slave mơ hệ thống động lực học không gian làm việc Sử dụng phương pháp điều khiển kết hợp lực vị trí Từ Robot Master Slave kết hợp với cách sử dụng điều khiển đồng với tỷ lệ độ lớn thời gian trễ kênh truyền thơng Thêm vào đó, hệ số phương pháp điều khiển đồng lựa chọn độc lập ứng với giá trị cho Robot Master Slave Sử dụng phương pháp ổn định Lyapunov chứng minh hệ thống hai Robot đồng dạng với tỷ lệ độ lớn ổn định tiệm cận với thời gian trễ biến thiên Ngoài kết khả quan thu được, kết mô cho thấy số nhược điểm hạn chế là: Khi thay đổi giá trị độ trễ làm hệ thống xuất nhiễu Khi độ trễ lớn gây ổn định hệ thống Đây nhược điểm quan trọng cần khắc phục 5.2 Hướng phát triển Giải thiếu sót tồn nêu trên, thời gian trễ ảnh hưởng đến tính cân hệ thống Teleoperation, liệu bị truyền thông Cải thiện luật điều khiển tham số điều khiển tốt Đề xuất thuật toán điều khiển Nghiên cứu hệ với Robot Master nhiều Robot Slave (SMMS) với tỉ lệ khác 68 TÀI LIỆU THAM KHẢO A J Madhani, G Niemeyer, and Jr J K Salisbury (1998), “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 D Lee, O Martinez-Palafox, and M W Spong (2006), “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 I G Polushin, A Teyebi, and H J Marquez (July 4-8, 2005), “Stabilization scheme for force reflecting Teleoperation with time-varying communication delay base on IOS small gain theorem”, Presented at the 16th IFAC World Congr., Pargue, Czech Republic I G Polushin, P X Liu and C H Lung (2007), “A force-reflection algorithm for improved transparency in bilateral Teleoperation with communication delay”, IEEE/ASME Transactions on Mechatronics, Vol 12, No 3, pp.361-374 H C Cho and J H Park (2005), “Impedance control with variable damping for bilateral Teleoperation under time delay”, JSME Inter Journal, serial C, Vol 48, No.4, pp 695-703 H Khalil (1996), Nonlinear systems Prentice Hall, Upper Saddle River, NJ07458 Hinosuke Kawaga and Toru Namekirawa (2011) “ Bilateral control of Nonlinear Teleoperation with time varying Communication Delays ”, J Funda, and R P Paul (1991), “A symbolic teleoperator interface for timedelayed underwater robot manipulation”, In Ocean technologies and opportunities in the Pacific for the 90s, pp 1526-1533 69 L J Love and W J Book (2004), “Force reflecting Teleoperation with adaptive impedance control”, System, Man, and Cybernetics, Part B, IEEE Trans., Vol 34, Issue 1, pp 159-165 10 N Hogan (1985), “Impedance control: An approach to manipulation”, JSME Trans Journal of Dynamics System, Measurement, and Control, Vol 107, pp.1-24 11 Nam Duc Do and Toru Namekirawa (2011) “ Cooperative Control Bases on force-reflection with four channel Teleoperation system”, In 2011 50th IEEE conference on decision and control and European Control conference 4879-4884, Orlando, FL 12 S B Skaar and C F Ruoff (Eds) (1987), “Teleoperation and robotics in space”, Progress in astronautics and aeronautics, Vol 161, American Institute of Aeronautics and Astronautics 13 R Uhrich (1973), “Terminus controlled deep ocean manipulator”, OCEANS, 5, pp 301-304 14 Giáo trình robot cơng nghiệp – TS Phạm Đăng Phước, NXB giáo dục Việt Nam 15 Cơ sở Robot công nghiệp – GS.TSKH.Nguyễn Văn Khang, NXB giáo dục Việt Nam 16 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 17 Lý thuyết điều khiển phi tuyến, Nguyễn Doãn Phước, NXB Khoa học 70 PHỤ LỤC PHỤ LỤC A CÁC KHỐI CỦA CHƯƠNG TRÌNH MƠ PHỎNG Hình A.1.1 Khối điều khiển hệ thống SMSS Hình A.1.2 Khối Master 71 Hình A.1.3 Khối điều khiển robot Master Hình A.1.4 Khối Slave 72 Hình A.1.5 Khối điều khiển robot Slave Hình A.1.6 Khối Human Hình A.1.7 Khối Delay 73 PHỤ LỤC B CHƯƠNG TRÌNH MATLAB CHO HỆ THỐNG ĐIỀU KHIỂN Phụ lục đưa vài chương trình matlab sử dụng trình điều khiển File DDrobot function [sys,x0,str,ts] = Minirobot(t,x,u,flag,Mm1,Mm2,Rm,mtheta1,mtheta2) %2link DD-Robot ‚̀S-Function % The following outlines the general structure of an S-function % switch flag, %%%%%%%%%%%%%%%%%% % Initialization % %%%%%%%%%%%%%%%%%% case 0, [sys,x0,str,ts]=mdlInitializeSizes(mtheta1,mtheta2); %%%%%%%%%%%%%%% % Derivatives % %%%%%%%%%%%%%%% case 1, sys=mdlDerivatives(t,x,u,Mm1,Mm2,Rm); %%%%%%%%%% % Update % %%%%%%%%%% case 2, sys=mdlUpdate(t,x,u); %%%%%%%%%%% % Outputs % %%%%%%%%%%% case 3, sys=mdlOutputs(t,x,u); %%%%%%%%%%%%%%%%%%%%%%% % GetTimeOfNextVarHit % %%%%%%%%%%%%%%%%%%%%%%% 74 case 4, sys=mdlGetTimeOfNextVarHit(t,x,u); %%%%%%%%%%%%% % Terminate % %%%%%%%%%%%%% case 9, sys=mdlTerminate(t,x,u); %%%%%%%%%%%%%%%%%%%% % Unexpected flags % %%%%%%%%%%%%%%%%%%%% otherwise error(['Unhandled flag = ',num2str(flag)]); end % end sfuntmpl % %========================================================== =================== % mdlInitializeSizes % Return the sizes, initial conditions, and sample times for the S-function %========================================================== =================== % function [sys,x0,str,ts]=mdlInitializeSizes(mtheta1,mtheta2) % % call simsizes for a sizes structure, fill it in and convert it to a % sizes array % % Note that in this example, the values are hard coded This is not a % recommended practice as the characteristics of the block are typically % defined by the S-function parameters % sizes = simsizes; sizes.NumContStates = 4;%x1,x2,x3,x4 sizes.NumDiscStates = 0; 75 sizes.NumOutputs sizes.NumInputs = 4;%y1,y2,y3,y4: -> z1,z2,dq1,dq2; = 2;% - is Tau1,Tau sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; % at least one sample time is needed sys = simsizes(sizes); % % initialize the initial conditions % x0 = [mtheta1,mtheta2,0,0]; % % str is always an empty matrix % str = []; % % initialize the array of sample times % ts = [0 0]; % end mdlInitializeSizes % %========================================================== =================== % mdlDerivatives % Return the derivatives for the continuous states %========================================================== =================== % function sys=mdlDerivatives(t,x,u,Mm1,Mm2,Rm) M=[Mm1 Mm2*cos(x(1)-x(2));Mm2*cos(x(1)-x(2)) Rm]; C=[0 x(4)*Mm2*sin(x(1)-x(2));-x(3)*Mm2*sin(x(1)-x(2)) 0]; %M=[M1+2*R*cos(x(2)) M2+R*cos(x(2));M2+R*cos(x(2)) M2];% - day la cac ma tran %C=[-R*x(3)*x(4)*sin(x(2))R*x(4)*(x(4)+x(3))*sin(x(2));R*x(3)*x(3)*sin(x(2))];%- day la cac ma tran %tmp=inv(M)*(u-C-B-D);-la ma tran 2x1 -u is tau[2,1] 76 tmp=inv(M)*(u-C*[x(3);x(4)]); %ddq= M(-1)* (Tau - h(q,dq)) -Tinh gia toc ddb sys(1)=x(3); % dq1 sys(2)=x(4); % dq2 sys(3)=tmp(1); % ddq1 sys(4)=tmp(2); % ddq2 % end mdlDerivatives % %========================================================== =================== % mdlUpdate % Handle discrete state updates, sample time hits, and major time step % requirements %========================================================== =================== % function sys=mdlUpdate(t,x,u) sys = []; % end mdlUpdate % %========================================================== =================== % mdlOutputs % Return the block outputs %========================================================== =================== % 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 % end mdlOutputs % 77 %========================================================== =================== % mdlGetTimeOfNextVarHit % Return the time of the next hit for this block Note that the result is % absolute time Note that this function is only used when you specify a % variable discrete-time sample time [-2 0] in the sample time array in % mdlInitializeSizes %========================================================== =================== % function sys=mdlGetTimeOfNextVarHit(t,x,u) sampleTime =0; %=1 % Example, set the next hit to be one second later sys = []; % end mdlGetTimeOfNextVarHit % %========================================================== =================== % mdlTerminate % Perform any end of simulation tasks %========================================================== =================== % function sys=mdlTerminate(t,x,u) sys = []; % end mdlTerminate File Parameter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% qs1=-0*(pi/180); qs2=0*(pi/180); qm1=-0*(pi/180); qm2=0*(pi/180); 78 zm1=0; zm2=0.1414; zs1=0; zs2=0.2828; l1=0.2; l2=0.2; lm1= 0.128; lm2= 0.128; x=100; y=10; z=1; Kd=15; Kp=20; Kf=10; p=1; Tm=0; Ts=0; Tm=0.01; Ts=0.01; del_Tm=0.015; del_Ts=0.015; freq_m =0.3; freq_s =0.3; dTm=0.5; dTs=0.5; Mm1=0.00149; Mm2=-0.000764; Rm=0.000686; mtheta1=0; mtheta2=0; File in đồ thị figure(1); plot(ScopeData1x.time,ScopeData1x.signals.values); 79 grid on; h = legend('zm','zsmu',2); figure(2); plot(ScopeData1y.time,ScopeData1y.signals.values); grid on; h = legend('zm','zsmu',2); figure(3); plot(ScopeData5x.time,ScopeData5x.signals.values); grid on; h = legend('Fop','Fenv',2); figure(4); plot(ScopeData5y.time,ScopeData5y.signals.values); grid on; h = legend('Fop','Fenv',2); figure(5); plot(ScopeData2x.time,ScopeData2x.signals.values); grid on; h = legend('dzm','dzsmu',2); figure(6); plot(ScopeData2y.time,ScopeData2y.signals.values); grid on; h = legend('dzm','dzsmu',2); 80 ... nghiên cứu hệ thống Teleoperation nhiều chủ yếu tập trung điều khiển vị trí, vận tốc lực phản hồi Trong thực tác vụ robot Master robot Slave hệ thống Teleoperation việc điều khiển vị trí lực phản... KẾ ĐIỀU KHIỂN HỆ THỐNG SMSS TELEOPERATION Hình 4.1 Sơ đồ khối điều khiển hệ thống SMSS? ??……………………………….…56 Hình 4.2 Vị trí Robot Slave tương tác với mơi trường………………………… 63 Hình 4.3 Biểu đồ vị trí. .. IV:THIẾT KẾ ĐIỀU KHIỂN HỆ SMSS TELEOPERATION 54 4.1 Đối tượng điều khiển 54 4.2 Thiết kế hệ thống điều khiển PD kết hợp điều khiển lực 54 4.3.Phân tích tính ổn định hệ thống