Hệ thống điều khiển chuyển động robot bền vững với mô men nhiễu cản

HOÀNG QUỐC XUYÊN BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - HOÀNG QUỐC XUYÊN ĐIỀU KHIỂN & TỰ ĐỘNG HÓA HỆ THỐNG ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT BỀN VỮNG VỚI MÔ MEN NHIỄU CẢN LUẬN VĂN THẠC SĨ KHOA HỌC ĐIỀU KHIỂN & TỰ ĐỘNG HĨA KHỐ 2009 HÀ NỘI -2011 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI HOÀNG QUỐC XUYÊN HỆ THỐNG ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT BỀN VỮNG VỚI MÔ MEN NHIỄU CẢN Chuyên ngành: ĐIỀU KHIỂN & TỰ ĐỘNG HÓA LUẬN VĂN THẠC SĨ KHOA HỌC ĐIỀU KHIỂN & TỰ ĐỘNG HÓA NGƯỜI HƯỚNG DẪN KHOA HỌC : TS NGUYỄN MẠNH TIẾN Hà Nội – 2011 Luận Văn Tốt Nghiệp LỜI CAM ĐOAN Tác giả xin cam đoan trực tiếp thực toàn nội dung nghiên cứu luận văn khơng có gian lận hay chép Các nội dung nghiên cứu đảm bảo theo yêu cầu đặt đề cương luận văn thạc sỹ khoa học đăng ký Hà nội, ngày 25 tháng 09 năm 2011 Người cam đoan Hoàng Quốc Xuyên Luận Văn Tốt Nghiệp MỤC LỤC LỜI CAM ĐOAN 1  DANH MỤC CÁC KÝ HIỆU VIẾT TẮT 3  DANH MỤC CÁC BẢNG 3  DANH MỤC CÁC HÌNH VẼ 4  MỞ ĐẦU 6  CHƯƠNG 1- TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP 9  1 Khái quát chung Robot Công Nghiệp 9  Một số phương pháp điều khiển điển hình .16  1.3 Kết luận chương 25  CHƯƠNG – HỆ THỐNG ĐIỀU KHIỂN ĐỘC LẬP CÁC KHỚP 27  Tổng quan cấu trúc hệ thống điều khiển chuyển động cho robot 27  2 Mơ hình tổng qt hệ truyền động cho khớp độc lập robot .28  Hệ thống điều khiển độc lập khớp robot 32  Tổng hợp điều chỉnh cho hệ thống điều khiển khớp độc lập .34  2.5 Kết luận chương 40  CHƯƠNG – THUẬT TỐN NHẬN DẠNG VÀ BÙ MƠMEN NHIỄU CẢN .41  Thuật tốn nhận dạng mơmen nhiễu cản 41  Hệ thống điều khiển khớp độc lập với khâu nhận dạng bù mômen nhiễu cản 45  3.3 Kết luận chương 49  CHƯƠNG 4- XÂY DỰNG CẤU HÌNH HỆ THỐNG ĐIỀU KHIỂN CHO ROBOT SCARA 50  Mơ hình tốn học Robot SCARA 50  Xây dựng hệ thống điều khiển độc lập cho khớp robot SCARA 74  Kết mô 79  KẾT LUẬN VÀ KIẾN NGHỊ .89  TÀI LIỆU THAM KHẢO .90  PHỤ LỤC 92  Luận Văn Tốt Nghiệp DANH MỤC CÁC KÝ HIỆU VIẾT TẮT KÝ HIỆU, VIẾT TẮT NỘI DUNG DCB Disturbance Calculating Block DOB Disturbance Observer LSPB Linear Segment with Parabolic Blends PD Proportional-Derivative Controller DANH MỤC CÁC BẢNG Bảng 4.1 - Tham số Denavit – Hartenberg Robot SCARA 52  Bảng 4.2 - Ký hiệu tham số động lực học Robot Scara Serpent 61  Bảng 4.3 - Tham số khí robot 80  Bảng 4.4 - Tham số động truyền động khớp robot 80  Luận Văn Tốt Nghiệp DANH MỤC CÁC HÌNH VẼ Hình 1.1- Hình dạng điển hình Robot Cơng nghiệp .12  Hình 1.2 - Các dạng khớp Robot 13  Hình 1.3 - Robot kiểu tọa độ Đề (TTT) 14  Hình 1.4 - Robot kiểu tọa độ trụ (RTT) .14  Hình 1.5 - Robot kiểu tọa độ Cầu (RRR) (RRT) 15  Hình 1.6 - Robot hoạt động theo hệ tọa độ góc .15  Hình 1.7 - Sơ đồ cấu trúc hệ thống điều khiển momen tính tốn 17  Hình 1.8 - Sơ đồ cấu trúc hệ thống điều khiển PD bù trọng trường 20  Hình 1.9 - Sơ đồ khối hệ thống điều khiển Thích nghi tự chỉnh 24  Hình 1.10 – Sơ đồ khối hệ thống điều khiển Thích nghi theo mơ hình mẫu 25  Hình 2.1 - Sơ đồ cấu trúc hệ truyền động khớp độc lập 32  Hình 2.2 - Sơ đồ cấu trúc hệ thống điều khiển cho khớp độc lập 33  Hình 2.3 - Sơ đồ cấu trúc mạch vòng dòng điện 34  Hình 2.4 - Sơ đồ cấu trúc mạch vịng dòng điện thu gọn 35  Hình 2.5 - Sơ đồ cấu trúc mạch vịng tốc độ 36  Hình 2.6 - Sơ đồ cấu trúc mạch vịng tốc độ thu gọn 37  Hình 2.7 - Sơ đồ cấu trúc mạch vịng vị trí 38  Hình 2.8 - Sơ đồ cấu trúc mạch vịng vị trí thu gọn 39  Hình 3.1 - Sơ đồ ước lượng mơ men nhiễu cản 43  Hình 3.2 - Sơ đồ ước lượng mô men nhiễu cản có khâu lọc thơng thấp .43  Hình 3.3 - Sơ đồ ước lượng mô men nhiễu cản tương đương .44  Hình 3.4 - Sơ đồ điều khiển khớp robot độc lập với khâu nhận dạng bù mômen nhiễu cản 45  Hình 3.5 - Cấu trúc điều khiển bù theo mômen nhiễu cản 46  Hình 4.1 - Hình ảnh Robot Scara Serpent .50  Hình 4.2- Cách đặt hệ trục tọa độ cho khớp Robot SCARA 51  Hình 4.3 - Hình chiếu Robot SCARA xuống mặt phẳng xOy 60  Hình 4.4 - Sơ đồ điều khiển độc lập cho hai khớp robot .79  Luận Văn Tốt Nghiệp Hình 4.5 – Sai lệch vị trí khớp lượng đặt hàm STEP tải định mức 81  Hình 4.6 – Sai lệch vị trí khớp lượng đặt hàm STEP tải định mức 81  Hình 4.7 – Sai lệch vị trí khớp lượng đặt hàm STEP tải lần tải định mức 82  Hình 4.8 – Sai lệch vị trí khớp lượng đặt hàm STEP tải lần tải định mức 83  Hình 4.9 – Sai lệch vị trí khớp lượng đặt quỹ đạo LSPB, tải định mức 85  Hình 4.10 - Sai lệch vị trí khớp lượng đặt quỹ đạo LSPB, tải định mức .85  Hình 4.11 - Sai lệch vị trí khớp lượng đặt quỹ đạo LSPB tải lần tải định mức 86  Hình 4.12 - Sai lệch vị trí khớp lượng đặt quỹ đạo LSPB tải lần tải định mức 87  Luận Văn Tốt Nghiệp MỞ ĐẦU MỞ ĐẦU Ngày mục tiêu tăng suất lao động thơng qua tự động hóa tồn q trình sản xuất trở thành chiến lược quan trọng ngành kinh tế Trong dây chuyền sản xuất với mức độ tự động hóa cao, robot cơng nghiệp đóng vai trị quan trọng việc giảm cường độ lao động cho người lao động, tăng suất độ xác gia cơng, tăng chất lượng số lượng sản phẩm, góp phần vào làm giảm giá thành sản phẩm Theo định nghĩa viện Robot Mỹ Robot cấu trúc khí đa năng, lập trình được, thiết kế để di chuyển vật, phận, công cụ thiết bị đặc biệt thông qua chuyển động khác lập trình để thực nhiệm vụ khác Robot trở thành công cụ thiếu nhà máy, xí nghiệp có mức độ tự động hố cao Robot đảm nhận cơng việc khó khăn như: làm việc môi trường độc hại, nguy hiểm, môi trường có độ phóng xạ nhiệt độ cao Với nhà máy khí robot thực chức nạp phôi cho máy công cụ, sơn, hàn Ngồi Robot cịn ứng dụng nhiều lĩnh vực khoa học như: y tế, sinh học, địa chất đời sống như: bán hàng, trông coi Khả hoạt động Robot đảm bảo hệ thống khí thân bệ, cánh tay, khớp v.v, muốn cấu khí thực nhiệm vụ theo yêu cầu công nghệ Robot phải có hệ thống điều khiển Hệ thống chuyển động robot hệ thống có tính phi tuyến mạnh ràng buộc cao, tham số động lực học mơ men qn tính, khối lượng tải thường biến đổi khơng xác định xác Với điều khiển kinh điển khó đảm bảo độ xác chuyển động cao Ở robot có tỉ số truyền truyền lớn, ảnh hưởng ràng buộc khớp tính phi tuyến hệ thống chuyển động nhỏ Điều cho phép tách độc lập khớp robot Thành phần mô men phi tuyến ràng buộc hệ thống chuyển động coi tín hiệu nhiễu khớp Hệ thống điều khiển đơn giản dễ thực thực tế điều khiển chuyển động khớp thiết kế độc lập, không phụ thuộc vào tham số khớp lân cận Luận Văn Tốt Nghiệp MỞ ĐẦU Tuy nhiên, với robot thực tế, tỉ số truyền truyền có giá trị hữu hạn Trong trường hợp này, ràng buộc khớp tính phi tuyến cấu chuyển động đáng kể Thành phần mô men nhiễu cản biểu thị ràng buộc phi tuyến robot thay đổi khó xác định xác ảnh hưởng đến độ xác chuyển động khớp Việc giải vấn đề nêu nội dung yêu cầu Luận văn với tiêu đề: Hệ thống điều khiển chuyển động robot bền vững với mơ men nhiễu cản Nội dung Luận văn nghiên cứu phương pháp ước lượng mô men nhiễu cản khớp xây dựng hệ thống điều khiển tuyến tính độc lập với khâu bù mô men nhiễu cản nhằm khử ràng buộc khớp Phương pháp điều khiển đề xuất áp dụng cho hai khớp đầu robot SCARA Luận văn trình bày thành chương với nội dung chương tóm tắt sau: Chương - Tổng quan robot công nghiệp: Giới thiệu cách tổng quan lịch sử phát triển, định nghĩa khái niệm robot cơng nghiệp Nghiên cứu phân tích ưu, nhược điểm số phương pháp điều khiển robot Chương - Hệ thống điều khiển độc lập khớp: Xây dựng mơ hình động lực học tổng qt cho khớp độc lập Thiết lập sơ đồ điều khiển độc lập cho khớp với mạch vòng điều chỉnh dòng điện, mạch vòng điều chỉnh tốc độ động mạch vịng điều chỉnh vị trí khớp Tổng hợp mạch vòng điều chỉnh Chương - Thuật tốn nhận dạng bù mơmen nhiễu cản: Xây dựng thuật tốn nhận dạng mơmen nhiễu cản, Xây dựng hệ thống điều khiển phân ly khớp với khâu nhận dạng bù mômen nhiễu cản động truyền động khớp Chương - Xây dựng cấu hình hệ thống điều khiển cho Robot SCARA: Nghiên cứu cụ thể mơ hình Robot SCARA, Tính tốn mơ hình động học vị trí, xây dựng phương trình động lực học, xây dựng hệ thống điều khiển độc lập cho khớp robot SCARA Mô hệ thống điều khiển phân ly khớp với khâu nhận dạng bù mômen nhiễu cản Luận Văn Tốt Nghiệp MỞ ĐẦU Mặc dù cố gắng tìm hiểu, nghiên cứu để hoàn thành nội dung yêu cầu đặt ra, xong thời gian trình độ thân có hạn nên luận văn tránh khỏi thiếu sót Vì tác giả mong nhận bảo góp ý tận tình thầy cô giáo hội đồng bảo vệ phản biện để luận văn hồn thiện Tơi xin chân thành cảm ơn thầy giáo TS Nguyễn Mạnh Tiến tận tình hướng dẫn giúp đỡ tơi hoàn thành luận văn TÀI LIỆU THAM KHẢO [14] Nguyễn Mạnh Tiến, Bùi Thị Khánh Hòa Hệ Thống Điều Khiển Quỹ Đạo Robot Với Nhận Dạng Tham Số Cơ Học Tạp chí Khoa Học & Cơng Nghệ Các Trường Đại Học Kỹ Thuật Số 77-2010 TIẾNG ANH [1] Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo, Robotics: Modelling, Planning and Control, Springer, 2008 [2] Frank L Lewis, Dawson, and Chaouki T Abdallah Robot Manipulator Control: Theory and Practice Second Edition Marcel Dekker, Inc 2004 [3] John J Craig Introduction to Robotics: Mechanics and Control 2nd Edition, Prentice Hall 1989 [4] Mahdi Salman Alshamasin, Florin Ionescu, Riad Taha Al-Kasasbeh, Kinematic Modeling and Simulation of a SCARA Robot by Using Solid Dynamics and Verification by MATLAB/Simulink, European Journal of Scientific Research ISSN 1450-216X Vol.37 No.3 (2009), pp.388-405 [5] Mariko Mizuochi, Toshiaki Tsuji, Kouhei Ohnishi, Improvement of Disturbance Suppression Based on Disturbance Observer, Advanced Motion Control, 2006 9th IEEE International Workshop on Date:27-29 March 2006 [6] Mark W Spong, Seth Hutchinson, and M Vidyasagar, Robot Dynamics And Control John Wiley & Sons, Second Edition 2004 [7] R Kelly, V Santibanez and A Loria, Control of Robot Manipulators in Joint Space Springer 2005 [8] Taylan Das M., L Canan Dulger, Mathematical Modeling, Simulation and Experimental Verification of a SCARA Robot, Journal of Simulation Modelling Practice and Theory, Vol.13, (2005), pp 257-271 91 PHỤ LỤC PHỤ LỤC CÁC KHỐI CON TRONG SƠ ĐỒ MƠ PHỎNG HỆ THỐNG Khối mạch vịng dòng điện Dựa sơ đồ điều khiển phân ly khớp với khâu nhận dạng bù mômen nhiễu cản hình 4.4 ta xây dựng nên sơ đồ mô phỏng, tùy theo chức nhiệm vụ cần giải mà ta gom số khâu thành hệ (Subsystem) sau: Bắt đầu sơ đồ mạch vịng dịng điện khớp 1: Hình 5.1 Sơ đồ mơ mạch vịng dịng điện khớp Tương tự ta có sơ đồ mạch vịng dịng điện khớp là: Hình 5.2 Sơ đồ mơ mạch vòng dòng điện khớp Các tham số điều chỉnh tính theo cơng thức tổng hợp mục 2.4.1, phần tính tốn thực file.m matlab có tên PHỤ LỤC ThamsoDKkhopRobot.m (Tham khảo Phụ Lục) Với vòng điều khiển ngồi ta coi mạch vịng dịng điện thành khối có tên Current Loop (tức mạch vịng dòng điện) Như với mạch vòng dòng điện cho khớp ta có khối Current Loop 1, vịng dịng điện cho khớp ta có khối Current Loop 2 Khối tính tốn mơ men nhiễu cản Khi mơ ta phải tính tốn mơmen nhiễu cản để tạo tín hiệu nhiễu Để tính thành phần mơmen nhiễu phục vụ việc mô ta phải tiến hành mơ khớp đồng thời Các cơng thức tính tốn mơ men nhiễu cản mục 4.2.3 4.2.4 Phần tính tốn thực đoạn chương trình dạng file.m matlab có tên TinhMomenCan.m Phần tính mơ men nhiễu cản đánh dấu thành hệ hình đây: Hình 5.3 Khối tính mơmen nhiễu cản Khối tính mơmen nhiễu cản hình 5.3 có chức tạo tín hiệu nhiễu phục vụ cho cơng tác mơ ta khơng có nhiễu thật hệ thống Trên sơ đồ mơ khối tính mơmen nhiễu cản ký hiệu DCB (Disturbance calculating Block) Khối nhận dạng mô men nhiễu cản Trong thực tế điều khiển khớp độc lập ta phải nhận dạng mô men nhiễu cản dựa vào thông số dòng điện phần ứng tốc độ động truyền động khớp cần điều khiển, sơ đồ nhận dạng phân tích trong hình 3.3 sơ đồ mô thực matlab sau: PHỤ LỤC Hình 5.4 Khối nhận dạng mơmen nhiễu cản DOB1 Hình 5.5 Khối nhận dạng mômen nhiễu cản DOB2 Trên sơ đồ mô khối nhận dạng mômen nhiễu cản đánh dấu thành hệ có ký hiệu khối DOB Khối tạo quỹ đạo đặt Để tạo quỹ đạo đặt ta viết đoạn chương trình matlab dạng m-file có tên Tinhquydaodat.m Trong tốn mơ ta sử dụng quỹ đạo đặt có dạng tổ hợp hàm bậc bậc Để xác định quỹ đạo đặt ta phải nhập thông số tọa độ điểm đầu q0, tọa độ điểm cuối qc, thời gian di chuyển tc Trong điều khiển độc lập để tiện theo dõi góc di chuyển khớp ta tính tốn góc khớp điểm đầu điểm cuối cần đặt thông qua tọa độ điểm đầu điểm cuối Các tính tốn góc khớp tương ứng với vị trí điểm đầu điểm cuối thực đoạn chương trình tính động học ngược matlab dạng m-file có tên TinhDHN.m PHỤ LỤC Cuối để có quỹ đạo đặt ta cần nhập tham số menu đây: Hình 5.6 Menu nhập thông số cho quỹ đạo đặt Phần tử tạo quỹ đạo đặt ký hiệu sơ đồ khâu có tên Trajectory Với khớp ta có Trajectory qd1, với khớp ta có Trajectory qd2 Tiếp theo sơ đồ mơ trường hợp sau đây: + Hệ bỏ qua ảnh hưởng mômen nhiễu cản + Hệ khâu bù mơmen có ảnh hưởng mơmen nhiễu cản + Hệ có khâu bù mơmen có ảnh hưởng mơmen nhiễu cản Hình 5.7 Sơ đồ mô bỏ qua ảnh hưởng mômen nhiễu cản PHỤ LỤC Hình 5.8 Sơ đồ mơ khơng có khâu bù có ảnh hưởng mơmen nhiễu cản PHỤ LỤC Hình 5.9 Sơ đồ mơ có khâu bù có ảnh hưởng mơmen nhiễu cản PHỤ LỤC PHỤ LỤC CÁC CHƯƠNG TRÌNH TÍNH TỐN PHỤC VỤ MƠ PHỎNG Chương trình tính tốn tham số cho điều chỉnh Chương trình ThamsoDKkhopRobot.m có chức tính tốn tham số cho điều chỉnh từ tham số động tham số khí robot ThamsoDKkhopRobot.m % CHUONG TRINH TINH TOAN CAC THAM SO BO DIEU CHINH % CHO DONG CO KHOP VA KHOP % -% -% Cac tham so khop m1=1.9;% Khoi luong noi L1=0.25; % Chieu dai noi Jk1=0.098;% Momen quan tinh noi Jd1=0.0002033;% Momen quan tinh dong co Mo1=0;%momen khong tai khop Kg1=0.25; % He so truyen khop (Kg1=1/i1 = 0.01) Rd1=3.5;% Dien Tro phan ung Dong Co Ld1=1.3;% Dien cam phan ung Dong Co Id1=1.5;% Dong dien phan ung Dong Co Ke1=0.047;% he so suat dien dong Km1=Ke1;% he so momen Km10=Km1;% he so momen danh dinh Kb1=4.8; % He so khuyech dai chinh luu Ki1=5/(2.5*Id1); % He so khuyech dai sensor dong dien Kw1=1; % He so khuyech dai sensor toc Kp1=1; % He so khuyech dai sensor vi tri Tb1=0.0005;% Hang so thoi gian bo Bien Doi Ti1=0.0001;% Hang so thoi gian senso dong Tw1=0.005;% Hang so thoi gian senso toc Tp1=0.1;% Hang so thoi gian senso vi tri Td1=Ld1/Rd1; % -% Tinh toan cac tham so cho khop Jt1=Jd1+Kg1^2*((m1/4)*L1^2+Jk1); %Momen Quan tinh tong Jt10=Jt1; %Momen Quan tinh tong danh dinh Toi1=Tb1+Ti1;% Hang so thoi gian nho cua mach vong dong dien Tow1=2*Toi1+Tw1;% Hang so thoi gian Tong vong ho cua mach vong toc Top1=2*Tow1+Tp1;% Hang so thoi gian Tong vong ho cua mach vong vi tri Koi1=(Ki1*Kb1)/Rd1; % He so khuyech dai mach vong dong dien ho Kow1=(Kw1*Km1)/(Ki1*Jt1);% He so khuyech dai mach vong toc ho Kop1=(Kg1*Kp1)/Kw1;% He so khuyech dai mach vong vi tri ho % -% Tinh toan cac bo dieu chinh cho khop Kpr1=Td1/(2*Koi1*Toi1); % He so khuyech dai bo dieu khien PI1 (Kp) PHỤ LỤC Tir1=Td1;% Hang so thoi gian cho bo dieu khien PI1 (Ti) Rw1=1/(2*Kow1*Tow1);% Bo dieu khien toc khop Rp1=1/(2*Kop1*Top1);% Bo dieu khien vi tri khop % -%****************************************** % -% Cac tham so khop m2=0.93;% Khoi luong noi L2=0.15; % Chieu dai noi Jk2=0.0115;% Momen quan tinh noi Jd2=0.0005033;% Momen quan tinh dong co Mo2=0; %momen khong tai khop Kg2=0.25; % He so truyen khop (Kg2=1/i2=0.0045) Rd2=3.5;% Dien Tro phan ung Dong Co Ld2=1.3;% Dien cam phan ung Dong Co Id2=1.5;% Dong dien phan ung Dong Co Ke2=0.047;% he so suat dien dong dong co Km2=Ke2;% he so momen Km20=Km2;% he so momen danh dinh Kb2=4.8; % He so khuyech dai chinh luu Ki2=5/(2.5*Id2); % He so khuyech dai sensor dong dien Kw2=1; % He so khuyech dai sensor toc Kp2=1; % He so khuyech dai sensor vi tri Tb2=0.0005;% Hang so thoi gian bo Bien Doi Ti2=0.0001;% Hang so thoi gian senso dong Tw2=0.005;% Hang so thoi gian senso toc Tp2=0.1;% Hang so thoi gian senso vi tri Td2=Ld2/Rd2;% Hang so thoi gian co hoc % -% Tinh toan cac tham so cho khop Jt2=Jd2+Kg2^2*((m2/4)*L2^2+Jk2); %Momen Quan tinh tong Jt20=Jt2; %Momen Quan tinh tong danh dinh Toi2=Tb2+Ti2;% Hang so thoi gian nho cua mach vong dong dien Tow2=2*Toi2+Tw2;% Hang so thoi gian Tong vong ho cua mach vong toc Top2=2*Tow2+Tp2;% Hang so thoi gian Tong vong ho cua mach vong vi tri Koi2=(Ki2*Kb2)/Rd2; % He so khuyech dai mach vong dong dien ho Kow2=(Kw2*Km2)/(Ki2*Jt2);% He so khuyech dai mach vong toc ho Kop2=(Kg2*Kp2)/Kw2;% He so khuyech dai mach vong vi tri ho % -% Tinh toan cac bo dieu chinh cho khop Kpr2=Td2/(2*Koi2*Toi2); % He so khuyech dai bo dieu khien PI2 (Kp) Tir2=Td2;% Hang so thoi gian cho bo dieu khien PI2 (Ti) Rw2=1/(2*Kow2*Tow2);% Bo dieu khien toc Rp2=1/(2*Kop2*Top2);% Bo dieu khien vi tri % -% Tan so cat cua bo loc thong thap "g" g=10000; % PHỤ LỤC % Khoi luong tai dat vao tay robot m3=1.5;% Khoi luong noi m4=1.5;% Khoi luong noi m_tdm=m3+m4; % Khoi luong tai dinh muc % Khoi luong khop (m3) cong khoi luong khop (m4) % duoc coi la khoi luong tai dinh muc m_tdm %******************************************* Chương trình tính tốn mơmen nhiễu cản phục vụ tạo nhiễu mơ Chương trình TinhMomenCan.m có chức tính tốn mơmen nhiễu cản phục vụ việc tạo nhiễu mô %CHUONG TRINH TINH TOAN MOMEN NHIEU CAN %PHUC VU CHO VIEC TAO NHIEU MO PHONG function MC = TinhMomenCan(u) ThamsoDKkhopRobot;%lay cac tham so tu bo thong so ddqk1=u(1)*Kg1; %gia toc goc khop ty le (Kg) voi gia toc Dong Co dqk1=u(2)*Kg1; %toc goc khop qk2=u(3)*Kg2; %goc khop theta2 dqk2=u(4)*Kg2; %toc goc khop ddqk2=u(5)*Kg2; %gia toc goc khop c2=cos(qk2);% cos(theta2) s2=sin(qk2);% Sin(theta2) DH11=(m2+m_tdm)*L1^2+((m2/4)+m_tdm)*L2^2+Jk2+2*((m2/2)+m_tdm)*L1*L2*c2; H12=((m2/4)+m_tdm)*L2^2+Jk2+((m2/2)+m_tdm)*L1*L2*c2; H21=H12; DH22=m_tdm*L2^2; h112=-2*((m2/2)+m_tdm)*L1*L2*s2; h122=-((m2/2)+m_tdm)*L1*L2*s2; h211=((m2/2)+m_tdm)*L1*L2*s2; H=[DH11 H12 ; H21 DH22];%ma tran H V1=h112*dqk1*dqk2+h122*dqk2^2; V2=h211*dqk1^2; V=[V1; V2];%vecto V(Q,dQ) ddQ=[ddqk1;ddqk2];% Vecto Gia toc goc D=H*ddQ+V; % Vecto nhieu MO=[Mo1;Mo2];%vecto momen khop robot khong tai Kg=[Kg1 0; Kg2]; MC=Kg*D+MO;%vecto momen nhieu can end % - PHỤ LỤC Chương trình tạo quỹ đạo đặt LSPB Chương trình Tinhquydaodat.m có chức tính tốn xác định đường biểu diễn góc khớp theo thời gian từ vị trí ban đầu q0 đến vị trí cuối qc, sử dụng làm lượng đặt đầu vào cho mạch vịng vị trí % HAM TAO QUY DAO DAT DANG LSPB function q = Tinhquydaodat(u) % Cac bien dau vao cho ham tao quy dao t=u(1);% bien thoi gian thuc cua he thong q0=u(2);% Gia tri goc khop ban dau qf=u(3);% Gia tri goc khop ban cuoi tf=u(4);% Thoi gian di chuyen k=u(5);% He so chon van toc V=k*(qf-q0)/tf; % Tinh Van Toc theo cac bien dau vao tb=(q0-qf+V*tf)/V; % Tinh Thoi gian tang toc giam toc a=V/tb; % Tinh Gia Toc % Quy dao dat 2-1-2 theo cac khoang thoi gian t if (t>=0)&&(ttb)&&(t(tf-tb)&&(t