BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI Nguyễn Quang Thái ĐIỀU KHIỂN ĐỘNG HỌC TAY MÁY KHOAN LỖ NỔ MÌN TRONG THI CƠNG CÁC CƠNG TRÌNH NGẦM LUẬN VĂN THẠC SĨ KỸ THUẬT KỸ THUẬT CƠ ĐIỆN TỬ Hà Nội – Năm 2018 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI Nguyễn Quang Thái ĐIỀU KHIỂN ĐỘNG HỌC TAY MÁY KHOAN LỖ NỔ MÌN TRONG THI CƠNG CÁC CƠNG TRÌNH NGẦM Chun nghành: Cơ Điện Tử LUẬN VĂN THẠC SĨ KỸ THUẬT KỸ THUẬT CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC: TS NGUYỄN HỒNG THÁI Hà Nội – Năm 2018 LỜI CAM ĐOAN Tôi xin cam đoan nội dung trình bày luận văn kết nghiên cứu thân tơi, khơng có chép hay copy tác giả Tôi xin tự chịu trách nhiệm lời cam đoan Tác giả NGUYỄN QUANG THÁI i MỤC LỤC LỜI CAM ĐOAN I DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIÊT TẮT V DANH MỤC CÁC HÌNH VẼ DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ VII DANH MỤC CÁC BẢNG XI I ĐẶT VẤN ĐỀ II MỤC ĐÍCH NGHIÊN CỨU CỦA LUẬN VĂN III PHẠM VI NGHIÊN CỨU IV Ý NGHĨA KHOA HỌC VÀ THỰC TIỄN CỦA LUẬN VĂN 4.1 Ý nghĩa khoa học 4.2 Ý nghĩa thực tiễn V PHƯƠNG PHÁP NGHIÊN CỨU CỦA LUẬN VĂN VI NỘI DUNG CỦA LUẬN VĂN Chương TỔNG QUAN VỀ ROBOT KHOAN NỔ MÌN TRONG THI CƠNG CƠNG TRÌNH NGẦM .4 1.1 Giới thiệu tổng quan quy trình khoan nổ mìn thi cơng cơng trình ngầm tay máy Robot 1.2 Lịch sử trình phát triển khoan nổ mìn .6 1.3 Các loại xe tay máy Robot khoan nổ mìn đào hầm .7 1.4 Tình hình nghiên cứu nước nước 10 1.4.1 Tình hình nghiên cứu nước ngồi .10 1.4.2 Tình hình nghiên cứu nước 12 Kết luận chương .13 Chương PHÂN TÍCH ĐỘNG HỌC TAY MÁY 14 2.1 Mô tả cấu tạo nguyên lý thiết bị 14 2.1.1 Mô tả cấu tạo xe tự hành tay máy Robot 14 2.1.2 Cấu tạo nguyên lý hoạt động tay máy Robot 15 2.2 Lược đồ hóa cấu máy .16 2.3 Thiết lập hệ phương trình động học 17 ii 2.3.1 Phương trình biến đổi tọa độ hướng từ giá khoan đến hệ quy chiếu gắn xe 17 2.3.2 Phương trình xác định vị trí hướng lỗ khoan mặt gương 19 2.4 Xác định thông số biến khớp điều khiển tay may Robot thủy lực 21 2.5 Mô chuyển động .27 Kết luận chương .30 Chương TÍNH TOÁN LỰA CHỌN HỆ THỐNG XY LANH THỦY LỰC VÀ MỘT SỐ KHỚP 31 3.1 Đặt vấn đề 31 3.2 Mơ hình hố kết cấu dạng nguyên lý .31 3.2.1 Cụm khớp vai .31 3.2.2 Cụm cẳng tay Robot 32 3.2.3 Cụm khớp cổ tay Robot 33 3.2.4 Cụm xoay giá đỡ 34 3.3 Tính hành trình xy lanh khớp quay vai .35 3.3.1 Chuyển động gật gù 35 3.3.2 Chuyển động xoay 36 3.4 Tính tồn hành trình xy lanh khớp cổ tay 40 3.4.1 Chuyển động gật gù 40 3.4.2 Chuyển động xoay 43 3.5 Tính chọn xy lanh 48 3.5.1 Tính tốn áp lực lên xy lanh 48 3.5.2 Tính đường kính xy lanh 78 3.5.3 Tính lưu lượng 79 3.6 Phân tích lực tác động lên số khớp 80 3.6.1 Cụm khớp xoay lật vai 80 3.6.2 Cụm khớp xoay lật cổ tay 89 3.7 Tính tốn kết cấu chốt 98 3.8 Tính tốn kết cấu tai 100 iii Kết luận chương 102 Thông số xy lanh 102 Thông số khớp .103 KẾT LUẬN VÀ KIẾN NGHỊ .106 I Các kết luận văn .106 II Những đề xuất 106 TÀI LIỆU THAM KHẢO .107 PHỤ LỤC 109 iv DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIÊT TẮT Ký hiệu H1 H2 Hxy lanh L1 α β 1 ,  xA, yA, zA P P51 P511 P512 P52 P521 P522 γ1, γ2 P3 P31 P32 F1, F2 F12 F121 F122 µ c1 , c2 , c3 mB1 σ θ π F12x F12z X O , X 'O 1 Z O1 XB ZB R B1 d6.1, d4.1, d2.1 t6.2, t4.2, t2.2 Nội dung ý nghĩa : Khoảng cách từ khớp vai đến khớp xy lanh : Khoảng cách chốt xy lanh : Hành trình xy lanh : Khoảng cách từ khớp vai đến khớp cầu : Góc gật gù : Góc xoay : Chiều dài xy lanh (1), (2) : Tọa độ điểm A theo hệ trục Oxyz : Hợp lực tác dụng lên khâu : Trọng lực khâu động : Thành phần lực dọc trục khâu P51 : Thành phần lực vng góc khâu P51 : Trọng lực khâu cố định : Thành phần lực dọc trục khâu P52 : Thành phần lực vng góc khâu P52 : Góc hợp phương trọng lực phương thành phần lực vng góc khâu động khâu cố định : Trọng lực xy lanh : Trọng lực khâu động xy lanh : Trọng lực khâu cố định xy lanh : Áp lực tác động lên xy lanh (1) (2) : Hợp lực xy lanh (1) (2) : Thành phần lực dọc trục khâu F12 : Thành phần lực vng góc khâu F12 : Góc tạo phương F12 trục khâu : Khoảng cách từ B1 đến điểm đặt lực P, P51, P52 : Tổng mô men quay tâm B1 : Mặt phẳng chứa xy lanh (3.1) : Góc tạo xy lanh (3.1) trục O3xσ : Mặt phẳng chiếu vng góc cấu cẳng tay : thành phần lực song song với trục O1xπ F12 : Thành phần lực song song với trục O1zπ F12 : Lực phản lực gối đỡ O1 : Thành phần lực vng góc với lưc phản lực O1 : Lực gối đỡ B1 : Thành phần lực vng góc với lưc phản lực B1 : Lực tác động lên chốt cụm : Đường kính chốt cụm 6, 4, : Độ dày tai cụm 6, 4, v δ6.2, δ4.2, δ2.2 t6.4, t4.4, t2.4 d6.3, d4.3, d2.3 δ6.4, δ4.4, δ2.4  6.4 ,  4.4 ,  2.4 h6.4, h4.4, h2.4 b1 ,b2 ,b3 mD σd [σd] σk [σk] τ [τc] Mu Wu [σu]tai ΣFk Q V1,V2 MM L B δ χ δm, δb Z k : Độ dày vành tai cụm 6, 4, : Độ dày tai cụm 6, 4, : Đường kính chốt cụm ,4 ,2 : Độ dày vành tai cụm 6, 4, : Khoảng cách tai cụm 6, 4, : Chiều cao chốt cụm 6, 4, : Khoảng cách hai chốt cụm 6, 4, : Tổng mô men tác động lên khớp D : Ứng suất dập : Ứng suất dập cho phép vật liệu : Ứng suất kéo : Ứng suất kéo cho phép vật liệu : Ứng suất cắt : Ứng suất cắt cho phép : Mô men uốn : Mô men chống uốn : Ứng suất uốn cho phép tai : Tổng lực tác động lên khâu, khớp : Lực tác động lên chốt : Lực tác động đỡ : Momen tác dụng lên : Chiều dài đỡ : Chiều rộng đỡ : Chiều dày đỡ : Hệ số ngoại lực : Độ mềm chi tiết bulông : Số bulông : Hệ số an tồn vi DANH MỤC CÁC HÌNH VẼ DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ Nội dung Trang Hình 1.1 Mơ tả quy trình khoan nổ mìn đào hầm Hình 1.2 Hộ chiếu nổ mìn mặt gương [14] Hình 1.3 Lịch sử phát triển khoan nổ mìn qua giai đoạn [6] Hình 1.4 Tay khoan Robot thủy lực hãng Atlas Copco tay khoan [25] Hình 1.5 Xe Robot tự hành thủy lực hãng Atlas Copco tay khoan [26] Hình 1.6 Tay khoan Robot thủy lực hãng Atlas Copco tay khoan [27] Hình 1.7 Sơ dồ bố trí lỗ khoan nổ mìn đường hầm Vouli [16] 11 Hình 1.8 Giải pháp khoan neo bu long [21] 11 Hình 1.9 Phân tích kết cấu tay máy Robot phương pháp phần tử hữu hạn [14] 12 Hình 2.1 Xe tự hành ba tay máy hãng AtlasCopco [26] 14 Hình 2.2 Xe di chuyển đường hầm nhỏ hẹp [26] 14 Hình 2.3 Tay máy khoan nổ mìn 15 Hình 2.4 Sơ đồ nguyên lý hoạt động tay máy 16 Hình 2.5 Các cụm khớp thiết kế dạng mạch vịng 17 Hình 2.6 Tọa độ D-H cho tay máy khoan lỗ nổ mìn bậc tự 18 Hình 2.7 Vị trí tương quan hệ quy chiếu điểm chuẩn “0” mặt gương hệ quy chiếu gắn xe 20 Hình 2.8 Quỹ đạo khâu thao tác 22 Hình 2.9 Cụm khớp vai 23 Hình 2.10 Tọa độ D-H cho xy lanh truyền động cụm khớp vai 24 Hình 2.11 Cụm khớp cổ tay 25 Hình 2.12 Tọa độ D-H cho xy lanh truyền động cụm cẳng tay 26 27 Hình 2.13 Hình 2.14 Đồ thị biến khớp tay máy hoạt động 28 Hình 2.15 Quy trình mơ chuyển động Robot 29 Hình 2.16 Giao diện lập trình Visual Studio 2012 29 Hình 2.17 Mơ q trình khoan tay máy Robot 30 Hình 3.1 Cơ cấu cụm khớp vai tay máy Robot 31 Hình 3.2 Sơ đồ ngun lí cấu xoay, lật khớp vai tay máy Robot 32 Hình 3.3 Cơ cấu cụm cẳng tay tay máy Robot 32 Hình 3.4 Sơ đồ nguyên lý cụm cấu cẳng tay 33 Hình 3.5 Cụm cấu khớp cổ tay Robot 33 Hình 3.6 Sơ đồ ngun lí cấu xoay, lật khớp cổ tay máy Robot 34 Hình 3.7 Mơ hình khơng gian giá đỡ 34 vii Hình 3.8 Sơ đồ nguyên lí cấu cụm xoay giá đỡ Hình 3.9 Chuyển động gật gù chiếu trục Oxz Hình 3.10 Mặt phẳng AO2O3 Hình 3.11 Mơ hình hóa chuyển động xoay trái phải cấu Hình 3.12 Mơ hình hóa cấu vị trí (1) Hình 3.13 Mơ hình hóa cấu vị trí (2) Hình 3.14 Chuyển động gật gù khơng gian Oxyz Hình 3.15 Mối quan hệ   Hình 3.16 Độ dài  12 góc đạt giá trị  max Hình 3.17 Độ dài  12 max góc đạt giá trị  Hình 3.18 Mặt phẳng AO2O3 Hình 3.19 Chuyển động xoay khơng gian Oxyz Hình 3.20 Xác định tọa độ điểm A góc đạt giá trị  max Hình 3.21 Cơ cấu góc xoay trái  max góc đạt giá trị  max Hình 3.22 Cơ cấu chiếu lên mặt phẳng Oxy Hình 3.23 Chuyển động xoay khơng gian Oxyz Hình 3.24 Cơ cấu chiếu lên mặt phẳng Oxy Hình 3.25 Biểu diễn lực tác dụng lên khâu Hình 3.26 Các thành phần tác dụng lên khâu bỏ hai xy lanh Hình 3.27 Biểu diễn lực tác dụng lên xy lanh Hình 3.28 Biểu diễn lực tác dụng lên khâu trường hợp αmax Hình 3.29 Phân tích lực tác động lên khâu trường hợp αmax Hình 3.30 Lực tác dụng lên cấu trường hợp αmin Hình 3.31 Phân tích lực tác dụng lên khâu trường hợp αmin mặt phẳng π Hình 3.32 Mặt phẳng B2NB3 trường hợp gật gù Hình 3.33 Biểu diễn lực tác dụng lên cấu không gian trường hợp cao gần Hình 3.34 Phân tích lực tác động lên khâu trường hợp αmax mặt phẳng π Hình 3.35 Lực tác dụng lên cấu trường hợp αmin Hình 3.36 Phân tích lực tác dụng lên khâu trường hợp αmin mặt phẳng π Hình 3.37 Mặt phẳng O2AO3 trường hợp xoay Hình 3.38 Độ thị phân bố áp lực độ dài xy lanh trường hợp β = Hình 3.39 Độ thị phân bố áp lực độ dài xy lanh trường hợp α = αmax Hình 3.40 Độ thị phân bố áp lực độ dài xy lanh trường hợp α = αmin Hình 3.41 Sơ đồ thu gọn lực T R Hình 3.42 Sơ đồ lực thu gọn Hình 3.43 Sơ đồ tính tốn lực đẩy xy lanh Hình 3.44 Biểu diễn lực tác dụng lên khớp lật cổ tay Hình 3.45 Các thành phần lực tác dụng lên khâu bỏ hai xy lanh viii 34 35 36 37 38 39 40 41 41 42 42 43 44 45 45 46 47 48 49 49 50 51 52 53 54 55 55 56 56 57 59 59 60 60 61 61 62 63 Y1=[diem1(i); diem2(i)]; a1=inv(X1)*Y1; % a=[a0 a1]' toado{i}=a1(1)+ a1(2).*t1; %toado1=x, toado2=y, toado3=z end %% huong cua diem tac dong cuoi %% quay theo goc Cardan % quay quanh x goc ax % quay quanh y goc ay % quay quanh z goc az k=pi/(18*t1e); for i=1:1:length(t1) ax(i)=-pi/10; ay(i)=-pi/2; az(i)=pi/2; end %chia khoang for i=1:1:10 x{i}=toado{1}(1,20*(i-1)+1:20*i); y{i}=toado{2}(1,20*(i-1)+1:20*i); z{i}=toado{3}(1,20*(i-1)+1:20*i); anpha{i}=ax(1,20*(i-1)+1:20*i); beta{i}=ay(1,20*(i-1)+1:20*i); gama{i}=az(1,20*(i-1)+1:20*i); end for i=1:1:10 for j=1:1:200 vt1(j)=x{1}(1); vt2(j)=y{1}(1); vt3(j)=z{1}(1); vt4(j)=anpha{1}(1); vt5(j)=beta{1}(1); vt6(j)=gama{1}(1); dx{i}(j)=x{i}(20); dy{i}(j)=y{i}(20); dz{i}(j)=z{i}(20); dax{i}(j)=anpha{i}(20); day{i}(j)=beta{i}(20); daz{i}(j)=gama{i}(20); end end toado_x=vt1; toado_y=vt2; toado_z=vt3; huong_x=vt4; huong_y=vt5; huong_z=vt6; for i=1:1:10 toado_x=[toado_x x{i} dx{i}]; 115 toado_y=[toado_y y{i} dy{i}]; toado_z=[toado_z z{i} dz{i}]; huong_x=[huong_x anpha{i} dax{i}]; huong_y=[huong_y beta{i} day{i}]; huong_z=[huong_z gama{i} daz{i}]; end %ghi vao file fid=fopen('quydao2_1_cardan.txt','w'); for i=1:1:2400 fprintf(fid,'%f %f %f %f %f %f\n',toado_x(i),toado_y(i),toado_z(i),huong_x(i),huong_y(i),huong_z(i)); end fclose(fid); CHƯƠNG TRÌNH THIẾT KẾ QUỸ ĐẠO ĐOẠN clc; clear all; %toa tam duong tron: tam=[0 3500 400]; %ban kinh duong tron R=2000; %goc nghieng khoan apha= pi/18; % vao (1) (quy dao bac 1) %thoi gian t10=0.01; t1e=1; t1=t10:0.01:t1e; angle1=apha/length(t1); %%%%%%%%%% start point %%%%%%%%% diem1=[2000 tam(2) 200]; %%%%%%%%%% end point %%%%%%%%% diem2=[2000 tam(2) 400]; %% Vi tri diem tac dong cuoi for i=1:1:3 %%%%%% cac he so cua duong bac X1=[1 t10; 116 : f=a0 + a1.t t1e]; Y1=[diem1(i); diem2(i)]; a1=inv(X1)*Y1; % a=[a0 a1 a2 a3]' toado{i}=a1(1)+ a1(2).*t1; %toado1=x, toado2=y, toado3=z end %% huong cua diem tac dong cuoi %% quay theo goc Cardan % quay quanh x goc ax % quay quanh y goc ay % quay quanh z goc az k=pi/(18*t1e); for i=1:1:length(t1) ax(i)=0; ay(i)=0; az(i)=pi/2; end %chia khoang fid=fopen('quydao2_2_cardan.txt','w'); for i=1:1:100 fprintf(fid,'%f %f %f %f %f %f\n',toado{1}(i),toado{2}(i),toado{3}(i),ax(i),ay(i),az(i)); end fclose(fid); CHƯƠNG TRÌNH THIẾT KẾ QUỸ ĐẠO ĐOẠN clc; clear all; %toa tam duong tron: tam=[0 3500 400]; %ban kinh duong tron R=2000; %goc nghieng khoan apha= pi/18; % thoi gian quay het vong t0=0; te=1.79; t=t0:0.01:te; %%%%%%%%%%%5 %%% vi tri diem tac dong cuoi k=pi/1.79; for i=1:1:length(t) %%%%%%5 toado_x(i)=tam(1)+R*cos(t(i)*k); toado_y(i)=tam(2); toado_z(i)=tam(3)+R*sin(t(i)*k); end 117 %% huong cua diem tac dong cuoi %% quay theo goc Cardan % quay quanh x goc ax % quay quanh y goc ay % quay quanh z goc az for i=1:1:length(t) ax(i)=0; ay(i)=0; az(i)=pi/2; end %chia khoang for i=1:1:9 x{i}=toado_x(1,20*(i-1)+1:20*i); y{i}=toado_y(1,20*(i-1)+1:20*i); z{i}=toado_z(1,20*(i-1)+1:20*i); anpha{i}=ax(1,20*(i-1)+1:20*i); beta{i}=ay(1,20*(i-1)+1:20*i); gama{i}=az(1,20*(i-1)+1:20*i); end for i=1:1:9 for j=1:1:200 vt1(j)=x{1}(1); vt2(j)=y{1}(1); vt3(j)=z{1}(1); vt4(j)=anpha{1}(1); vt5(j)=beta{1}(1); vt6(j)=gama{1}(1); dx{i}(j)=x{i}(20); dy{i}(j)=y{i}(20); dz{i}(j)=z{i}(20); dax{i}(j)=anpha{i}(20); day{i}(j)=beta{i}(20); daz{i}(j)=gama{i}(20); end end toado_x=vt1; toado_y=vt2; toado_z=vt3; huong_x=vt4; huong_y=vt5; huong_z=vt6; for i=1:1:9 toado_x=[toado_x x{i} dx{i}]; toado_y=[toado_y y{i} dy{i}]; toado_z=[toado_z z{i} dz{i}]; 118 huong_x=[huong_x anpha{i} dax{i}]; huong_y=[huong_y beta{i} day{i}]; huong_z=[huong_z gama{i} daz{i}]; end %ghi vao file fid=fopen('quydao2_3_cardan.txt','w'); for i=1:1:2180 fprintf(fid,'%f %f %f %f %f %f\n',toado_x(i),toado_y(i),toado_z(i),huong_x(i),huong_y(i),huong_z(i)); end fclose(fid); CHƯƠNG TRÌNH THIẾT KẾ QUỸ ĐẠO ĐOẠN clc; clear all; %toa tam duong tron: tam=[0 3500 400]; %ban kinh duong tron R=2000; %goc nghieng khoan apha= pi/18; % vao (1) (quy dao bac 1) %thoi gian t10=0.01; t1e=1; t1=t10:0.01:t1e; angle1=apha/length(t1); %%%%%%%%%% start point %%%%%%%%% diem1=[-2000 tam(2) 400]; %%%%%%%%%% end point %%%%%%%%% diem2=[-1500 tam(2) 400]; %% Vi tri diem tac dong cuoi for i=1:1:3 %%%%%% cac he so cua duong bac X1=[1 t10; t1e]; Y1=[diem1(i); diem2(i)]; 119 : f=a0 + a1.t a1=inv(X1)*Y1; % a=[a0 a1 a2 a3]' toado{i}=a1(1)+ a1(2).*t1; %toado1=x, toado2=y, toado3=z end %% huong cua diem tac dong cuoi %% quay theo goc Cardan % quay quanh x goc ax % quay quanh y goc ay % quay quanh z goc az k=pi/(18*t1e); for i=1:1:length(t1) ax(i)=0; ay(i)=0; az(i)=pi/2; end fid=fopen('quydao2_4_cardan.txt','w'); for i=1:1:100 fprintf(fid,'%f %f %f %f %f %f\n',toado{1}(i),toado{2}(i),toado{3}(i),ax(i),ay(i),az(i)); end fclose(fid); CHƯƠNG TRÌNH THIẾT KẾ QUỸ ĐẠO ĐOẠN clc; clear all; %toa tam duong tron: tam=[0 3500 400]; %ban kinh duong tron R=1500; %goc nghieng khoan apha= pi/18; % thoi gian quay het vong t0=0; te=1.79; t=t0:0.01:te; %%%%%%%%%%%5 %%% vi tri diem tac dong cuoi k=pi/1.79; for i=1:1:length(t) %%%%%%5 toado_x(i)=tam(1)-R*cos(t(i)*k); toado_y(i)=tam(2); toado_z(i)=tam(3)+R*sin(t(i)*k); end %% huong cua %% quay theo % quay quanh % quay quanh diem tac dong cuoi goc Cardan x goc ax y goc ay 120 % quay quanh z goc az for i=1:1:length(t) ax(i)=0; ay(i)=0; az(i)=pi/2; end %chia khoang for i=1:1:9 x{i}=toado_x(1,20*(i-1)+1:20*i); y{i}=toado_y(1,20*(i-1)+1:20*i); z{i}=toado_z(1,20*(i-1)+1:20*i); anpha{i}=ax(1,20*(i-1)+1:20*i); beta{i}=ay(1,20*(i-1)+1:20*i); gama{i}=az(1,20*(i-1)+1:20*i); end for i=1:1:9 for j=1:1:200 vt1(j)=x{1}(1); vt2(j)=y{1}(1); vt3(j)=z{1}(1); vt4(j)=anpha{1}(1); vt5(j)=beta{1}(1); vt6(j)=gama{1}(1); dx{i}(j)=x{i}(20); dy{i}(j)=y{i}(20); dz{i}(j)=z{i}(20); dax{i}(j)=anpha{i}(20); day{i}(j)=beta{i}(20); daz{i}(j)=gama{i}(20); end end toado_x=vt1; toado_y=vt2; toado_z=vt3; huong_x=vt4; huong_y=vt5; huong_z=vt6; for i=1:1:9 toado_x=[toado_x x{i} dx{i}]; toado_y=[toado_y y{i} dy{i}]; toado_z=[toado_z z{i} dz{i}]; huong_x=[huong_x anpha{i} dax{i}]; huong_y=[huong_y beta{i} day{i}]; 121 huong_z=[huong_z gama{i} daz{i}]; end %ghi vao file fid=fopen('quydao2_5_cardan.txt','w'); for i=1:1:2180 fprintf(fid,'%f %f %f %f %f %f\n',toado_x(i),toado_y(i),toado_z(i),huong_x(i),huong_y(i),huong_z(i)); end fclose(fid); CHƯƠNG TRÌNH THIẾT KẾ QUỸ ĐẠO ĐOẠN clc; clear all; %toa tam duong tron: tam=[0 3500 400]; %ban kinh duong tron R=2000; %goc nghieng khoan apha= pi/18; % vao (1) (quy dao bac 1) %thoi gian t10=0.01; t1e=1; t1=t10:0.01:t1e; angle1=apha/length(t1); %%%%%%%%%% start point %%%%%%%%% diem1=[1500 tam(2) 400]; %%%%%%%%%% end point %%%%%%%%% diem2=[0 tam(2) 400]; %% Vi tri diem tac dong cuoi for i=1:1:3 %%%%%% cac he so cua duong bac X1=[1 t10; t1e]; : f=a0 + a1.t Y1=[diem1(i); diem2(i)]; a1=inv(X1)*Y1; % a=[a0 a1 a2 a3]' toado{i}=a1(1)+ a1(2).*t1; %toado1=x, toado2=y, toado3=z end 122 %% huong cua diem tac dong cuoi %% quay theo goc Cardan % quay quanh x goc ax % quay quanh y goc ay % quay quanh z goc az k=pi/(18*t1e); for i=1:1:length(t1) ax(i)=0; ay(i)=0; az(i)=pi/2; end %ghi vao file fid=fopen('quydao2_6_cardan.txt','w'); for i=1:1:100 fprintf(fid,'%f %f %f %f %f %f\n',toado{1}(i),toado{2}(i),toado{3}(i),ax(i),ay(i),az(i)); end fclose(fid); CHƯƠNG TRÌNH THIẾT KẾ QUỸ ĐẠO ĐOẠN clc; clear all; %toa tam duong tron: tam=[0 3500 900]; %ban kinh duong tron R=500; %goc nghieng khoan apha= pi/18; % thoi gian quay het vong t0=0; te=0.59; t=t0:0.01:te; %%%%%%%%%%%5 %%% vi tri diem tac dong cuoi k=3*pi/(0.59*2); for i=1:1:length(t) %%%%%%5 toado_x(i)=tam(1)+R*sin(t(i)*k); toado_y(i)=tam(2); toado_z(i)=tam(3)-R*cos(t(i)*k); end %% huong cua %% quay theo % quay quanh % quay quanh diem tac dong cuoi goc Cardan x goc ax y goc ay 123 % quay quanh z goc az for i=1:1:length(t) ax(i)=0; ay(i)=0; az(i)=pi/2; end %chia khoang for i=1:1:3 x{i}=toado_x(1,20*(i-1)+1:20*i); y{i}=toado_y(1,20*(i-1)+1:20*i); z{i}=toado_z(1,20*(i-1)+1:20*i); anpha{i}=ax(1,20*(i-1)+1:20*i); beta{i}=ay(1,20*(i-1)+1:20*i); gama{i}=az(1,20*(i-1)+1:20*i); end for i=1:1:3 for j=1:1:200 vt1(j)=x{1}(1); vt2(j)=y{1}(1); vt3(j)=z{1}(1); vt4(j)=anpha{1}(1); vt5(j)=beta{1}(1); vt6(j)=gama{1}(1); dx{i}(j)=x{i}(20); dy{i}(j)=y{i}(20); dz{i}(j)=z{i}(20); dax{i}(j)=anpha{i}(20); day{i}(j)=beta{i}(20); daz{i}(j)=gama{i}(20); end end toado_x=vt1; toado_y=vt2; toado_z=vt3; huong_x=vt4; huong_y=vt5; huong_z=vt6; for i=1:1:3 toado_x=[toado_x x{i} dx{i}]; toado_y=[toado_y y{i} dy{i}]; toado_z=[toado_z z{i} dz{i}]; huong_x=[huong_x anpha{i} dax{i}]; huong_y=[huong_y beta{i} day{i}]; 124 huong_z=[huong_z gama{i} daz{i}]; end %ghi vao file fid=fopen('quydao2_7_cardan.txt','w'); for i=1:1:860 fprintf(fid,'%f %f %f %f %f %f\n',toado_x(i),toado_y(i),toado_z(i),huong_x(i),huong_y(i),huong_z(i)); end fclose(fid); CHƯƠNG TRÌNH THIẾT KẾ QUỸ ĐẠO ĐOẠN clc; clear all; %toa tam duong tron: tam=[0 3500 400]; %ban kinh duong tron R=2000; %goc nghieng khoan apha= pi/18; % vao (1) (quy dao bac 1) %thoi gian t10=0.01; t1e=1; t1=t10:0.01:t1e; angle1=apha/length(t1); %%%%%%%%%% start point %%%%%%%%% diem1=[-500 tam(2) 900]; %%%%%%%%%% end point %%%%%%%%% diem2=[0 tam(2) 900]; %% Vi tri diem tac dong cuoi for i=1:1:3 %%%%%% cac he so cua duong bac X1=[1 t10; t1e]; : f=a0 + a1.t Y1=[diem1(i); diem2(i)]; a1=inv(X1)*Y1; % a=[a0 a1 a2 a3]' toado{i}=a1(1)+ a1(2).*t1; %toado1=x, toado2=y, toado3=z end 125 %% huong cua diem tac dong cuoi %% quay theo goc Cardan % quay quanh x goc ax % quay quanh y goc ay % quay quanh z goc az k=pi/(18*t1e); for i=1:1:length(t1) ax(i)=0; ay(i)=0; az(i)=pi/2; end %ghi vao file fid=fopen('quydao2_8_cardan.txt','w'); for i=1:1:100 fprintf(fid,'%f %f %f %f %f %f\n',toado{1}(i),toado{2}(i),toado{3}(i),ax(i),ay(i),az(i)); end fclose(fid); 10 CHƯƠNG TRÌNH TÍNH TỐN CÁC BIẾN KHỚP clc; clear all; format long; dof=7;% so bac tu t0=0; te=2; t=t0:0.01:2; %%%%%%%%%%% Toa suy rong %%%%%%%%%%%% syms q1 q2 q3 q4 q5 q6 q7 ax ay az; %%%%%%%%%%%%%% Bang DH %%%%%%%%%%%%%%% DH=[q1 500 200 pi/2; q2 0 pi/2; pi/2 q3 pi/2; q4 35 70 -pi/2; q5 155 -pi/2; q6 620 pi/2; q7 800 750 0]; %%%%%%%%%%% Ma tran CARDAN %%%%%%%%%%% cardan=[cos(ay)*cos(az),-cos(ay)*sin(az),sin(ay); cos(ax)*sin(az)+cos(az)*sin(ax)*sin(ay),cos(ax)*cos(az)sin(ax)*sin(ay)*sin(az),-cos(ay)*sin(ax); sin(ax)*sin(az)cos(ax)*cos(az)*sin(ay),cos(az)*sin(ax)+cos(ax)*sin(ay)*sin(az),cos(ax)*c os(ay)]; %%%%%%%%%%%%%% Ma tran cua khau cuoi so voi khau goc %%%%%%%%%%% A=eye(4); for i=1:1:dof Rz=[cos(DH(i,1)) -sin(DH(i,1)) 0; sin(DH(i,1)) cos(DH(i,1)) 0; 126 0 Tz=[1 0 0 0 0 0 0; 1]; 0; 0; DH(i,2); 1]; Tx=[1 0 DH(i,3); 0; 0 0; 0 1]; Rx=[1 0 0; cos(DH(i,4)) -sin(DH(i,4)) 0; sin(DH(i,4)) cos(DH(i,4)) 0; 0 1]; A=A*Rz*Tz*Tx*Rx; end %%%%%%%%% tinh nghiem q7 %%%%%%%%% for i=1:1:length(t) Q7(i)=pi/2; end %%%%%%%%%%%% vi tri va huong khau cuoi fid=fopen('quydao1_cardan.txt','r'); for i=1:1:length(t) xE(i)=fscanf(fid,'%f',1) yE(i)=fscanf(fid,'%f',1) zE(i)=fscanf(fid,'%f',1) cd_x(i)=fscanf(fid,'%f',1) cd_y(i)=fscanf(fid,'%f',1) cd_z(i)=fscanf(fid,'%f',1) end fclose(fid) %%%%%%%% tinh toan cac bien khop bang Newton-Raphson %%%%%%%%%% nghiem dau %%%%%%%%%%%% nghiem=[1.570796 1.775944 1594.483201 1.570796 -1.165649 1.570796 1.570796]'; for i=1:1:length(t) sCD=subs(cardan,[ax;ay;az],[cd_x(i);cd_y(i);cd_z(i)]); f1=A(1,4)-xE(i); f2=A(2,4)-yE(i); f3=A(3,4)-zE(i); f4=A(1,1)-sCD(1,1); f5=A(2,2)-sCD(2,2); f6=A(3,3)-sCD(3,3); f7=q7-Q7(i); F=[f1;f2;f3;f4;f5;f6;f7]; J=jacobian(F,[q1;q2;q3;q4;q5;q6;q7]); 127 sF=subs(F,[q1;q2;q3;q4;q5;q6;q7],nghiem); sF=double(sF) e=1e-6; M=20; dem=0; while((norm(sF)> e) && (dem