BÀI TẬP GIỮA KỲ ROBOT CÔNG NGHIỆP đề tài KHẢO SÁT ROBOT 6 BẬC TỰ DO 5 KHỚP QUAY VÀ 1 KHỚP TỊNH TIẾN

Tieuluanmoi skkndowloadbychat@gmail.com ĐẠI HỌC BÁCH KHOA HÀ NỘI TRƯỜNG CƠ KHÍ o0o BÀI TẬP GIỮA KỲ ROBOT CÔNG NGHIỆP ĐỀ TÀI: KHẢO SÁT ROBOT BẬC TỰ DO: KHỚP QUAY VÀ KHỚP TỊNH TIẾN Giảng viên hướng dẫn : TS Trần Thị Thanh Hải Học viên : Ninh Ngọc Quý Mã số học viên : 20212132M Tieuluanmoi skkndowloadbychat@gmail.com LỜI NÓI ĐẦU Hiện nay, khoa học kỹ thuật robot nói chung robot cơng nghiệp nói riêng phát triển mạnh mẽ đặc biệt nước có kinh tế phát triển, góp phần mang lại hiệu kinh tế cao Việc nghiên cứu chế tạo robot công nghiệp để ứng dụng sản xuất cần thiết Với đời robot thay cho người làm việc giúp cho việc thực cơng việc địi hỏi độ xác cao mà người khó làm Việc sử dụng robot công nghiệp nhà máy sản xuất làm tăng suất, nâng cao chất lượng sản phẩm Nhận thấy vai trò tầm quan trọng robot vây, nước ta này, robot đầu tư nghiên cứu nhiều, đặc biệt trường đại học có Trường Đại học Bách khoa Hà Nội “Robot Công Nghiệp” học phần bắt buộc chương trình Module 3: Robot - Cơ điện tử, học phần giúp cho sinh viên bước đầu làm quen với việc thiết kế khí hệ thống Robot bản, ứng dụng phần mềm CAD 2D/3D, mô nguyên lý hoạt động robot, giúp cho sinh viên nắm vững kiến thức học phần làm quen với nhiệm vụ người kỹ sư Có thể nói học phần thiếu sinh viên chuyên ngành Cơ điện tử Được hướng dẫn giúp đỡ nhiệt tình TS Trần Thị Thanh Hải, em hoàn thành đề tài giao “Khảo sát robot bậc tự do: khớp quay, khớp tịnh tiến” Bởi thời gian kiến thức có giới hạn, khơng tránh khỏi sai sót ngồi ý muốn Do em mong bảo đóng góp ý kiến thầy để đề tài hoàn thiện Em xin chân thành cảm ơn hướng dẫn TS Trần Thị Thanh Hải giúp em hoàn thành đề tài Hà Nội, tháng năm 2022 Ninh Ngọc Quý Tieuluanmoi skkndowloadbychat@gmail.com Mục lục LỜI NÓI ĐẦU CHƯƠNG I: PHÂN TÍCH NGUN LÝ VÀ THƠNG SỐ KỸ THUẬT 1.1 Nhiệm vụ thiết kế 1.2 Các thành phần kết cấu robot CHƯƠNG II: TÍNH TỐN THIẾT KẾ VÀ KHẢO SÁT KHÔNG GIAN LÀM VIỆC 2.1 Thiết lập sơ đồ động robot 2.2 Giải toán động học thuận 2.2.1 Các ma trận biến đổi 2.2.2 Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác 2.2.3 Thiết lập ma trận trạng thái khâu thao tác theo cấu trúc động học 2.2.4 Giải toán động học thuận CHƯƠNG III: XÁC ĐỊNH KHÔNG GIAN LÀM VIỆC CỦA ROBOT 10 11 3.1 Lập trình chương trình Matlab 11 3.2 Kết đạt 15 Tieuluanmoi skkndowloadbychat@gmail.com CHƯƠNG I: PHÂN TÍCH NGUN LÝ VÀ THƠNG SỐ KỸ THUẬT 1.1 Nhiệm vụ thiết kế - Robot bậc tự do: khớp quay khớp tịnh tiến - Các kích thước khâu biến khớp phù hợp yêu cầu đề 1.2 Các thành phần kết cấu robot - Thân robot: Là khâu cố định, đặt thẳng đứng giữ robot cố định làm việc, gắn với khâu động qua khớp quay với trục z01 thẳng đứng - Khâu 1: Khâu dẫn động nằm ngang vng góc với trục thẳng đứng suốt q trình làm việc robot, có khả quay xung quanh trục z01 qua khớp quay - Khâu 2: Khâu động có khả quay mặt phẳng vuông trục thẳng đứng qua khớp quay nối với khâu - Khâu 3: trục vít me - đai ốc bi (trục vít tịnh tiến, đai ốc quay) - Khâu 4, 5, 6: Khớp quay cổ tay robot dùng để định hướng điểm thao tác cuối không gian Tieuluanmoi skkndowloadbychat@gmail.com CHƯƠNG II: TÍNH TỐN THIẾT KẾ VÀ KHẢO SÁT KHÔNG GIAN LÀM VIỆC 2.1 Thiết lập sơ đồ động robot Hình 2.1 Sơ đồ động học Robot Sử dụng phương pháp Denavit-Hartenberg để giải toán động học robot Thiết lập hệ tọa độ Denavit-Hartenberg: Bảng D-H: Link θ d a α q1 d1 п/2 q2 d2 п/2 q3 0 q4 d4 п/2 q5 0 - п/2 q6 d6 0 Tieuluanmoi skkndowloadbychat@gmail.com Ta chọn: d1 = 3; d2 = 2; d4 = 1; d5 = 0; d6 = 0.5 2.2 Giải toán động học thuận Đối với toán động học thuận, vị trí, vận tốc, gia tốc biến khớp xem biết, cần tìm vị trí, vận tốc, gia tốc khâu thao tác hệ tọa độ cố định Vị trí khâu thao tác hệ tọa độ cố định xác định tọa độ thao tác gồm tọa độ định vị điểm tác động cuối hướng khâu thao tác Chú ý: Từ nay, để đơn giản biểu thức, ta quy ước sin = s, cos = c 2.2.1 Các ma trận biến đổi Ta chọn: d1 = 3; d2 = 2; d4 = 1; d5 = 0; d6 = 0.5 Áp dụng chương trình Matlab ta có ma trận biến đổi sau: A_01 = [cos(q1), 0, sin(q1), 0] [sin(q1), 0, -cos(q1), 0] [ 0, 1, 0, 3] [ 0, 0, 0, 1] [cos(q2), 0, sin(q2), 0] [sin(q2), 0, -cos(q2), 0] [ 0, 1, 0, 2] [ 0, 0, 0, 1] A_12 = A_23 = [1, 0, 0, 0] [0, 1, 0, 0] [0, 0, 1, q3] [0, 0, 0, 1] Tieuluanmoi skkndowloadbychat@gmail.com A_34 = [cos(q4), 0, sin(q4), 0] [sin(q4), 0, -cos(q4), 0] [ 0, 1, 0, 1] 0, 0, 1] [cos(q5), 0, -sin(q5), 0] [sin(q5), 0, cos(q5), 0] [ 0, -1, 0, 0] [ 0, 0, 0, 1] [ 0, A_45 = A_56 = [cos(q6), -sin(q6), 0, 0] [sin(q6), cos(q6), 0, 0] [ 0, 0, 1, 1/2] [ 0, 0, 0, 1] Từ ta có: A_06 = [ c(q6)*(c(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)) + c(q1)*s(q2)*s(q5)) + s(q6)*(c(q4)*s(q1) - c(q1)*c(q2)*s(q4)), c(q6)*(c(q4)*s(q1) - c(q1)*c(q2)*s(q4)) - s(q6)*(c(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)) + c(q1)*s(q2)*s(q5)), c(q1)*c(q5)*s(q2) - s(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)), 2*s(q1) + c(q1)*s(q2) - (s(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)))/2 + q3*c(q1)*s(q2) + (c(q1)*c(q5)*s(q2))/2] [- c(q6)*(c(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)) - s(q1)*s(q2)*s(q5)) - s(q6)*(c(q1)*c(q4) + c(q2)*s(q1)*s(q4)), s(q6)*(c(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)) - s(q1)*s(q2)*s(q5)) c(q6)*(c(q1)*c(q4) + c(q2)*s(q1)*s(q4)), s(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)) + Tieuluanmoi skkndowloadbychat@gmail.com c(q5)*s(q1)*s(q2), s(q1)*s(q2) - 2*c(q1) + (s(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)))/2 + q3*s(q1)*s(q2) + (c(q5)*s(q1)*s(q2))/2] [ s(q2)*s(q4)*s(q6), c(q4)*c(q5)*s(q2)) - c(q6)*s(q2)*s(q4), c(q4)*s(q2)*s(q5), - c(q6)*(c(q2)*s(q5) - c(q4)*c(q5)*s(q2)) s(q6)*(c(q2)*s(q5) - c(q2)*c(q5) - (c(q2)*c(q5))/2 - q3*c(q2) - (c(q4)*s(q2)*s(q5))/2 - c(q2)] [ 0, 0, 0, 1] Tieuluanmoi skkndowloadbychat@gmail.com 2.2.2 Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác Lấy gốc tọa độ thiết lập khâu thao tác O3 trùng với E Sử dụng góc Cardan xác định hướng vật rắn Gọi p vector tọa độ định vị khâu thao tác p=[x y z α β γ ]T Trong đó: yE zE Tọa độ vị trí điểm tác động cuối: r0 =[x Hướng khâu tác động cuối: −cos β sin γ cos β cos γ [ c11 (α , β , γ ) γ) R0 (α , β , γ )= c E c12(α , β , ][ c13(α , β , γ ) (α , β , γ ) c (α , β , γ ) c 21 c31 (α , β , γ ) c32 (α , β , γ ) c33 (α , β , γ ) (α , β , γ ) = 22 23 sin α sin β cos γ +cos α sin γ −sin α sin β sin γ +cos α −cos α sin β cos γ +sin α sin γ cos α sin β sin γ +sin α Tieuluanmoi skkndowloadbychat@gmail.com Khi đó, ma trận biến đổi tọa độ khâu cuối biểu diễn tọa độ khâu thao tác là: [ R0 r ] 2.2.3 Thiết lập ma trận trạng thái khâu thao tác theo cấu trúc động học Ma trận biến đổi tọa độ biểu diễn trạng thái khâu thao tác xác định từ cấu trúc động học robot Ma trận đưa dạng sau; [ A0(q) r 0(q) Trong đó: q=[q q q ] ]T [ a11(q ) a12 (q) 10 Tieuluanmoi skkndowloadbychat@gmail.com a13 (q) ] A0= a 21(q ) a22 a31(q ) a32 (q) (q) a23 a33 (q) (q) 11 Tieuluanmoi skkndowloadbychat@gmail.com r0=[ x( q) y (q) z (q)]T Phương trình động học robot nhận dạng ma trận sau: T 0(q)=T 0( p) n [ A0(q) r0(q) [ n R0 ( α , β , γ ) r0 ( x ,y ,z ) ] ] Từ có ta có phương trình động học dạng ma trận có dạng khai triển sau: a12 (q) a13 (q) c12 (α , β , γ ) [ a11 (q) x (q) c13(α , β , γ ) ] [ c11 (α , β ,γ) xE ] 2.2.4 Giải toán động học thuận a Xác định tọa độ định vị điểm tác động cuối: Các tọa độ định vị điểm tác động cuối xác định so sánh phần tử vế hệ phương trình động học dạng ma trận (*), ta có: xE =x (q) yE = y (q) 12 Tieuluanmoi skkndowloadbychat@gmail.com zE =z (q) ⇨ XE = 2*s(q1) + c(q1)*s(q2) - (s(q5)*(s(q1)*s(q4) + c(q1)*c(q2)*c(q4)))/2 + q3*c(q1)*s(q2) + (c(q1)*c(q5)*s(q2))/2 ⇨ ⇨ YE = s(q1)*s(q2) - 2*c(q1) + (s(q5)*(c(q1)*s(q4) - c(q2)*c(q4)*s(q1)))/2 + q3*s(q1)*s(q2) + (c(q5)*s(q1)*s(q2))/2 ⇨ ZE = - (c(q2)*c(q5))/2 - q3*c(q2) - (c(q4)*s(q2)*s(q5))/2 - c(q2)] b Xác định hướng khâu thao tác Hướng khâu thao tác xác định cách đối chiếu phần tử không hàng (cột) hai ma trận quay phương trình (*): 13 Tieuluanmoi skkndowloadbychat@gmail.com c11(α , β , γ )=a11(q) => c13(α , β , γ )=a13(q )=> c23 (α , β , γ )=a23 ( q) c o s β c o s γ = a 1 ( q ) s i n β = a ( q ) s i n α c o s β = a ( q ) CHƯƠNG III: XÁC ĐỊNH KHÔNG GIAN LÀM VIỆC CỦA ROBOT 3.1 Lập trình chương trình Matlab Chương trình gồm chương trình 14 Tieuluanmoi skkndowloadbychat@gmail.com a) Chương trình 1: Ma_tran_truyen.m b) Chương trình chính: Vẽ khơng gian làm việc robot Ta nhận thấy tọa độ khâu cuối 4, 5, xác định hướng khâu thao tác nên để giảm bớt tính tốn, ta coi tọa độ khâu không đổi trình vẽ Ta đặt giới hạn khớp khâu sau: min_q1 min_q2 min_q3 min_q4 min_q5 min_q6 = = = = = = 0; 0; 0; 0; 0; 0; max_q1 max_q2 max_q3 max_q4 max_q5 max_q6 = = = = = = 2*pi; 2*pi; 2; 2*pi; 2*pi; 2*pi; 15 Tieuluanmoi skkndowloadbychat@gmail.com assume(a1,'real');assume(a1>0); assume(a2,'real');assume(a2>0); assume(a3,'real');assume(a3>0); assume(a4,'real');assume(a4>0); assume(a5,'real');assume(a5>0); assume(a6,'real');assume(a6>0); assume(d1,'real');assume(d1>0); assume(d2,'real');assume(d2>0); assume(d3,'real');assume(d3>0); assume(d4,'real');assume(d4>0); assume(d5,'real');assume(d5>0); assume(d6,'real');assume(d6>0); assume(q1,'real'); assume(q2,'real'); assume(q3,'real'); assume(q4,'real'); assume(q5,'real'); assume(q6,'real'); assume(alpha1,'real'); assume(alpha2,'real'); assume(alpha3,'real'); assume(alpha4,'real'); assume(alpha5,'real'); assume(alpha6,'real'); q = [q1;q2;q3;q4;q5;q6]; %% Bang D-H % theta d a alpha % q1 d1 pi/2 % q2 d2 pi/2 % q3 0 % q4 d4 pi/2 % q5 0 -pi/2 % q6 d6 0 %A = Ma_tran_truyen(theta, a, d, alpha) A_01 = Ma_tran_truyen(theta1, a1, d1, alpha1) A_12 = Ma_tran_truyen(theta2, a2, d2, alpha2) A_23 = Ma_tran_truyen(theta3, a3, d3, alpha3) A_34 = Ma_tran_truyen(theta4, a4, d4, alpha4) A_45 = Ma_tran_truyen(theta5, a5, d5, alpha5) A_56 = Ma_tran_truyen(theta6, a6, d6, alpha6) A_06 = A_01*A_12*A_23*A_34*A_45*A_56 R_06 = A_06(1:3, 1:3); %% Thay so %d1 = 3; d2 = 2; d4 = 1; d5 = 0; d6 = 0.5 A_06 = subs(A_06,{theta1 theta2 theta3 theta4 theta5 a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5 q6 q3 0.5 0 0 0 pi/2 pi/2 pi/2 -pi/2 T_06 = A_06(1:3,4); A_01 = subs(A_01,{theta1 theta2 theta3 theta4 theta5 a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5 q6 q3 0.5 0 0 0 pi/2 pi/2 pi/2 -pi/2 theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) 16 Tieuluanmoi skkndowloadbychat@gmail.com A_12 = subs(A_12,{theta1 theta2 theta3 theta4 theta5 a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5 q6 q3 0.5 0 0 0 pi/2 pi/2 pi/2 -pi/2 A_23 = subs(A_23,{theta1 theta2 theta3 theta4 theta5 a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5 q6 q3 0.5 0 0 0 pi/2 pi/2 pi/2 -pi/2 A_34 = subs(A_34,{theta1 theta2 theta3 theta4 theta5 a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5 q6 q3 0.5 0 0 0 pi/2 pi/2 pi/2 -pi/2 A_45 = subs(A_45,{theta1 theta2 theta3 theta4 theta5 a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5 q6 q3 0.5 0 0 0 pi/2 pi/2 pi/2 -pi/2 A_56 = subs(A_56,{theta1 theta2 theta3 theta4 theta5 a1 a2 a3 a4 a5 a6 alpha1 alpha2 alpha3 alpha4 alpha5 q6 q3 0.5 0 0 0 pi/2 pi/2 pi/2 -pi/2 theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) %% Ve khong gian thao tac min_q1 = 0; min_q2 = 0; min_q3 = 0; min_q4 = 0; min_q5 = 0; min_q6 = 0; max_q1 = 2*pi; max_q2 = 2*pi; max_q3 = 2; max_q4 = 2*pi; max_q5 = 2*pi; max_q6 = 2*pi; step1 = (max_q1 step2 = (max_q2 step3 = (max_q3 step4 = (max_q4 step5 = (max_q5 step6 = (max_q6 - min_q1)/90; min_q2)/90; min_q3)/10; min_q4)/2; min_q5)/2; min_q6)/2; i = 1; xE(1,:) = 0; yE(1,:) = 0; zE(1,:) = 0; for i1 = min_q1:step1:max_q1 for i2 = min_q2:step2:max_q2 for i3 = min_q3:step3:max_q3 %for i4 = min_q4:step4:max_q4 %for i5 = min_q5:step5:max_q5 %for i6 = min_q6:step6:max_q6 T = subs(T_06,{q1 q2 q3 q4 q5 q6},{i1 i2 i3 0 0}); xE(1,i) = double(T(1,1)); yE(1,i) = double(T(2,1)); zE(1,i) = double(T(3,1)); %plot3(xE(i),yE(i),zE(i),'b.') %pause(0.01) i = i+1; %end %end 17 Tieuluanmoi skkndowloadbychat@gmail.com 18 Tieuluanmoi skkndowloadbychat@gmail.com 3.2 Kết đạt Hình 3.1 Khơng gian làm việc khơng gian 3D 19 Tieuluanmoi skkndowloadbychat@gmail.com Hình 3.2 Mặt phẳng làm việc XY 20 Tieuluanmoi skkndowloadbychat@gmail.com Hình 3.3 Mặt phẳng làm việc XZ 21 ... theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5 0}) theta6 d1 d2 d3 d4 d5 d6 alpha6},{q1 q2 q4 q5... A_ 45 = Ma_tran_truyen(theta5, a5, d5, alpha5) A_ 56 = Ma_tran_truyen(theta6, a6, d6, alpha6) A_ 06 = A_ 01* A _12 *A_23*A_34*A_ 45* A_ 56 R_ 06 = A_ 06( 1: 3, 1: 3); %% Thay so %d1 = 3; d2 = 2; d4 = 1; d5 =... %for i5 = min_q5:step5:max_q5 %for i6 = min_q6:step6:max_q6 T = subs(T_ 06, {q1 q2 q3 q4 q5 q6},{i1 i2 i3 0 0}); xE (1, i) = double(T (1, 1)); yE (1, i) = double(T(2 ,1) ); zE (1, i) = double(T(3 ,1) ); %plot3(xE(i),yE(i),zE(i),'b.')