Điều khiển robot hàn hồ quang đồ gá thiết bị di trượt

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - LƯU QUỐC KHÁNH ĐIỀU KHIỂN ROBOT HÀN HỒ QUANG – ĐỒ GÁ – THIẾT BỊ DI TRƯỢT Chuyên ngành : Kỹ thuật Cơ điện tử LUẬN VĂN THẠC SĨ KHOA HỌC KỸ THUẬT CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC: PGS.TS Bùi Văn Hạnh Hà Nội – Năm 2018 LUẬN VĂN THẠC SĨ 09/2018 MỞ ĐẦU Theo lịch sử phát triển Robot, tay máy công nghiệp đạt thành công lớn nước phát triển thời gian dài (Mỹ, Nhật, Đức …) , sử dụng nhiều dây chuyền sản xuất tiên tiến Đi theo tiến lồi người, Robot nói chung tay máy nói riêng ngày ứng dụng rộng rãi phổ biến nhiều lĩnh vực : công nghiệp, nghiên cứu phát triển, quân sự, y học, khoa học vũ trụ, sinh hoạt đời thường… Robot nói chung tay máy nói riêng ngày đòi hỏi cao độ tin cậy, độ xác, khả phối hợp, khả tự ứng xử thơng minh, khả đa dụng… Vì tiềm phát triển, cần thiết Robot giới đại, để sử dụng kiến thức chuyên ngành đào tạo trường học nên em định tiếp cận với robot dạng Kuka đề tài Luận văn thạc sĩ Với hướng dẫn PGS.TS Bùi Văn Hạnh – Bộ môn Hàn Cơng nghệ kim loại, em tìm hiểu thực tế robot DWE-V1 Phịng Thí nghiệm Cơng nghệ hàn C8-108 Sau q trình tìm hiểu robot DWE-V1, với định hướng PGS.TS Bùi Văn Hạnh, em nhận đề tài Luận văn : “ Điều khiển robot hàn hồ quang, đồ gá thiết bị di trượt ” LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 LỜI CẢM ƠN Em xin chân thành cảm ơn thầy cô môn Hàn Công nghệ kim loại – Viện Cơ Khí – Trường Đại học Bách Khoa Hà Nội tạo điều kiện cho em suốt trình thực Luận văn thạc sĩ Đặc biệt PGS.TS Bùi Văn Hạnh – Bộ môn Hàn Cơng nghệ kim loại tận tình hướng dẫn giúp đỡ em suốt thời gian qua Mặc dù em cố gắng nỗ lực để hồn thành tốt Luận văn thạc sĩ Nhưng em khơng tránh khỏi thiếu sót Nên em mong nhận ý kiến, nhận xét, đóng góp đánh giá q thầy Em xin chân thành cảm ơn ! LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 MỤC LỤC MỞ ĐẦU .1 Chương : TỔNG QUAN VỀ ĐỀ TÀI VÀ GIỚI THIỆU VỀ DWE-V1 1.1 Mục tiêu đề tài 1.2 Tổng quan robot 1.3 Giới thiệu robot DWE-V1 1.4.1 Các thông số thiết kế khâu DWE-V1 1.4.2 Cấu trúc động học DWE-V1 10 1.4.3 Cấu tạo hệ thống điều khiển cho DWE-V1 10 1.4.4 Phần mềm điều khiển 14 1.4.5 Đánh giá hoạt động DWE-V1 20 Chương : TÍNH TỐN LÍ THUYẾT ĐỘNG HỌC 21 2.1 Định vị định hướng vật rắn không gian 21 2.1.1 Ma trận cosin hướng 21 2.1.2 Các góc Euler 22 2.1.3 Các góc Roll – Pitch – Yaw 25 2.2 Bài toán động học thuận .28 2.2.1 Phương pháp Denavit – Hartenberg 29 2.2.2 Bài toán động học thuận vị trí 32 2.2.3 Sơ đồ động học DWE-V1 32 2.3 Bài toán động học ngược – phương pháp số 34 2.3.1 Ma trận Jacobian .34 2.3.2 Công thức tổng quát tính Jacobian cho robot chuỗi 34 LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 2.3.3 Tính ma trận Jacobian .36 2.3.4 Giải toán động học ngược cho DWE-V1 phương pháp số .38 2.4 Tính tốn động học robot không gian động 40 Chương : TÍNH TỐN LÍ THUYẾT ĐỘNG LỰC HỌC .47 CHO DWE-V1 47 3.1 Xây dựng mơ hình động lực học cho DWE-V1 47 3.2 Bài toán động lực học cho DWE-V1 .51 CHƯƠNG : KẾT QUẢ TÍNH TOÁN ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CHO DWE-V1 BẰNG PHƯƠNG PHÁP SỐ SỬ DỤNG PHẦN MỀM MATLAB 53 4.1 Tổng quan phần mềm MATLAB 53 4.1.1 Giới thiệu MATLAB 53 4.1.2 Giao diện người sử dụng 54 4.1.3 Đồ họa MATLAB 55 4.2 Các thông số động học DWE-V1 58 4.3 Kết tính tốn động học cho DWE-V1 phương pháp số sử dụng phần mềm Matlab 59 4.4 Kết tính tốn động lực học phương pháp số sử dụng phần mềm Matlab 63 LẬP TRÌNH MƠ PHỎNG 67 KẾT LUẬN .75 TÀI LIỆU THAM KHẢO .76 PHỤ LỤC 77 LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 Chương : TỔNG QUAN VỀ ĐỀ TÀI VÀ GIỚI THIỆU VỀ DWE-V1 1.1 Mục tiêu đề tài Một số mục tiêu cần đạt Đồ án :  Giới thiệu tổng quan DWE-V1, tay máy dạng Kuka, sản phẩm thiết kế, chế tạo lắp ráp Phịng Thí nghiệm Công nghệ hàn C8109  Nêu sở lí thuyết để tính tốn động học động lực học cho robot dạng Kuka theo phương pháp số với thông số thực tế DWE-V1  Nêu sở lí thuyết để tính tốn động học cho hệ thống đồ gá quay thiết bị di trượt  Lập trình tính tốn động học động lực học cho DWE-V1 theo phương pháp số phần mềm Matlab  Xuất kết tính toán động học động lực học cho DWE-V1 dạng file DAT, file MAT đồ thị thể kết tính tốn động học động lực học cho DWE-V1 1.2 Tổng quan robot Robot máy, thiết bị tự động, linh hoạt phục vụ cho người Robot có số đặc điểm đặc trưng sau : - Có hình dạng giống người cánh tay người - Có khả thao tác tự động - Có khả bắt chước thao tác giống người Robot máy, thiết bị cố định di động, tích hợp từ nhiều phận, có phận sau : - Cơ cấu chấp hành - Hệ thống dẫn động - Hệ thống điều khiển theo chương trình có khả lập trình linh hoạt - Hệ thống thơng tin giám sát LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 Trong năm gần việc áp dụng loại robot vào dây chuyền sản xuất ngày sử dụng rộng rãi doanh nghiệp Ví dụ : robot hàn, robot phun sơn công ty sản xuất lắp ráp ô tô, xe máy Đặc biệt công ty sản xuất lắp ráp linh kiện điện tử Đây cơng việc địi hỏi độ xác cao thường lặp lại nên dễ gây mệt mỏi cho người làm Vì việc sử dụng robot công ty phổ biến Các công việc môi trường độc hại, thường xun tiếp xúc với hóa chất robot biện pháp thay cho người 1.3 Giới thiệu robot DWE-V1 Robot DWE-V1 loại tay máy có bậc tự do, hoạt động linh hoạt, ứng dụng nhiều lĩnh vực nghiên cứu sản xuất công nghiệp Các khớp robot DWE-V1 khớp quay nên có hình dáng nhỏ gọn hoạt động ổn định Robot DWE-V1 có động đặt vị trí khớp quay Sơ đồ khớp quay robot DWE-V1 sau : LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 Hình 1.1 Sơ đồ khớp robot DWE-V1 Trục quay khớp 4, khớp khớp tay máy cắt điểm Đây đặc điểm quan trọng dạng cấu trúc động học yêu cầu gia công chế tạo lắp ráp tay máy hoàn chỉnh Robot hàn DWE-V1 bao gồm phận sau : - 06 khâu tay máy - 06 động AC Servo - 06 truyền giảm tốc : truyền bánh rang sóng, hộp số hành tinh - Đầu hàn gắn khâu - Vật liệu chế tạo nhôm - Các chi tiết liên kết truyền động phụ trợ thêm LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 Hình ảnh thực tế robot hàn DWE-V1 : Hình 1.2 Hình ảnh thực tế robot hàn DWE-V1 Phịng Thí nghiệm LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 09/2018 1.4.1 Các thông số thiết kế khâu robot hàn DWE-V1 Các cấu cụm chi tiết có cấp xác 5, chân đế có lớp bảo vệ đảm bảo tiêu chuẩn IP52 Tiêu chuẩn thiết kế theo tiêu chuẩn ISO Hình Mơ hình kết cấu robot hàn DWE-V1 Có dạng hình trụ, kích thước bao 456x257x350mm Khâu giá gia cơng đủ cứng vững cho tồn tay máy, lắp hộp số động Có đặt ổ đỡ chặn khâu giá Vật liêu gia công gang  Thơng số kích thước khâu : Có hình dạng phức tạp với kích thước bao 407.5x292.5x342.5 (mm) Khâu gia công đủ cứng vững để đỡ khâu phía nó, lắp hộp số, động Vật liệu gia công gang  Thông số kích thước khâu : Kích thước bao 630x131x125 (mm) Khâu gia công đủ cứng vững để đỡ khâu Vật liệu gia cơng gang  Thơng số kích thước khâu : LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang LUẬN VĂN THẠC SĨ 0 09/2018 0.0015]; % - KHAU -% m4=2; rc4_nga=transpose([0.00002 -0.00005 0.00002 1]); rc4=rc4_nga(1:3); I4=0.000001*[0.002 0 0.001 0 0 0.0015]; % - KHAU -% m5=1; rc5_nga=transpose([0.00003 0.00002 0.00005 1]); rc5=rc5_nga(1:3); I5=0.000001*[0.001 0 0.001 0 0 0.0015]; % - KHAU -% m6=0.3; rc6_nga=transpose([0.00004 0.00005 -0.00002 1]); rc6=rc6_nga(1:3); I6=0.000001*[0.001 0 0.002 0 0 0.003]; %%%%% TINH MA TRAN CHUYEN DOI %%%%% T01=[ cos(q1) sin(q1) sin(q1) -cos(q1) 0 d1 0 ]; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 78 LUẬN VĂN THẠC SĨ 09/2018 T12=[ cos(q2) -sin(q2) a2*cos(q2) sin(q2) cos(q2) a2*sin(q2) 0 0 0 ]; T23=[ cos(q3) sin(q3) a3*cos(q3) sin(q3) -cos(q3) a3*sin(q3) 0 0 ]; T34=[ cos(q4) -sin(q4) sin(q4) cos(q4) 0 -1 d4 0 ]; T45=[ cos(q5) sin(q5) sin(q5) -cos(q5) 0 00 0 ]; T56=[ cos(q6) -sin(q6) 0 sin(q6) cos(q6) 0 0 d6 0 ]; T02=T01*T12; T02=simplify(T02); T03=T02*T23; T03=simplify(T03); T04=T03*T34; T04=simplify(T04); T05=T04*T45; T05=simplify(T05); T06=T05*T56; T06=simplify(T06) %%%%% LAY RA MA TRAN COSIN CHI HUONG %%%%% LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 79 LUẬN VĂN THẠC SĨ 09/2018 z0=[0 1]; R01=T01(1:3,1:3); R02=T02(1:3,1:3); R03=T03(1:3,1:3); R04=T04(1:3,1:3); R05=T05(1:3,1:3); R06=T06(1:3,1:3); %%%%% TINH MA TRAN JACOBIAN %%%%% z1=R01*z0; z2=R02*z0; z3=R03*z0; z4=R04*z0; z5=R05*z0; p66=rc6; p56=R05*[0;0;d6]+R06*p66; p56=simplify(p56); p46=R04*[0;0;0]+p56; p36=R03*[0;0;d4]+p46; p36=simplify(p36); p26=R02*[a3*cos(q3);a3*sin(q3);0]+p36; p26=simplify(p26); p16=R01*[a2*cos(q2);a2*sin(q2);0]+p26; p16=simplify(p16); p06=T01(1:3,4)+p16; p06=simplify(p06); J11=cross(z0,p06); J21=z0; J11=simplify(J11); J1=[J11;J21]; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 80 LUẬN VĂN THẠC SĨ 09/2018 J12=cross(z1,p16); J22=z1; J12=simplify(J12); J2=[J12;J22]; J13=cross(z2,p26); J23=z2; J13=simplify(J13); J3=[J13;J23]; J14=cross(z3,p36); J24=z3; J14=simplify(J14); J4=[J14;J24]; J15=cross(z4,p46); J25=z4; J15=simplify(J15); J5=[J15;J25]; J16=cross(z5,p56); J26=z5; J16=simplify(J16); J6=[J16;J26]; J=[J1, J2, J3, J4, J5, J6] %%%%% GIAI BAI TOAN DONG HOC NGUOC BANG PHUONG PHAP LAP %%%%% %%%%% QUY DAO DIEM TAC DONG CUOI %%%%% clear t syms t t=[0:0.01:20]; xe=(350+50*sin(t)); ye=(350+50*cos(t)); LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 81 LUẬN VĂN THẠC SĨ 09/2018 ze=(350+5*t); plot3(xe,ye,ze) %%%%% CHUONG TRINH LAP %%%%% clear t syms t xe=0.001*(350+50*sin(t)); ye=0.001*(350+50*cos(t)); ze=0.001*(350+5*t); xt=transpose([xe ye ze 0 0]) %subs(xt,{t},0) xq=transpose([T06(1,4) T06(2,4) T06(3,4) 0 0]) %subs(xq,{q1,q2,q3,q4,q5,q6},{q10,q20,q30,q40,q50,q60}) syms t0 T N epsilon q01 q02 q03 q04 q05 q06 syms t u1 u2 u3 u4 u5 u6 c1 c2 c3 c4 c5 c6 syms q1cham q2cham q3cham q4cham q5cham q6cham %%%%% THU VOI BO DU LIEU DAU VAO %%%%% t0=0; T=20; N=2000; epsilon=0.000001; q01=0; q02=-pi/3; q03=pi/4; q04=0; q05=pi/6; q06=0; q0=transpose([q01 q02 q03 q04 q05 q06]) delta_t=T/N k=0 tk=t0 qk=q0 while (k0) kq_dhn_q(csh,csc)=kq_dhn_q(csh,csc)-2*pi*(floor(kq_dhn_q(csh,csc)/(2*pi))); else kq_dhn_q(csh,csc)=kq_dhn_q(csh,csc)+2*pi*abs(floor(kq_dhn_q(csh,csc)/(2*pi))); end end end kq_dhn_qcham(1,:)=[0 0 0 0] kq_dhn_qcham(2:(N+1),1)=diff(kq_dhn_q(1:(N+1),1)) kq_dhn_qcham(2:(N+1),2)=diff(kq_dhn_q(1:(N+1),2)) LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 83 LUẬN VĂN THẠC SĨ 09/2018 kq_dhn_qcham(2:(N+1),3)=diff(kq_dhn_q(1:(N+1),3)) kq_dhn_qcham(2:(N+1),4)=diff(kq_dhn_q(1:(N+1),4)) kq_dhn_qcham(2:(N+1),5)=diff(kq_dhn_q(1:(N+1),5)) kq_dhn_qcham(2:(N+1),6)=diff(kq_dhn_q(1:(N+1),6)) kq_dhn_qchamcham(1,:)=[0 0 0 0] kq_dhn_qchamcham(2:(N+1),1)=diff(kq_dhn_qcham(1:(N+1),1)) kq_dhn_qchamcham(2:(N+1),2)=diff(kq_dhn_qcham(1:(N+1),2)) kq_dhn_qchamcham(2:(N+1),3)=diff(kq_dhn_qcham(1:(N+1),3)) kq_dhn_qchamcham(2:(N+1),4)=diff(kq_dhn_qcham(1:(N+1),4)) kq_dhn_qchamcham(2:(N+1),5)=diff(kq_dhn_qcham(1:(N+1),5)) kq_dhn_qchamcham(2:(N+1),6)=diff(kq_dhn_qcham(1:(N+1),6)) save kq_dhn_q kq_dhn_q save kq_dhn_qcham kq_dhn_qcham save kq_dhn_qchamcham kq_dhn_qchamcham save kq_dhn_q.dat kq_dhn_q /ascii save kq_dhn_qcham.dat kq_dhn_qcham /ascii save kq_dhn_qchamcham.dat kq_dhn_qchamcham /ascii %%%%% TINH MA TRAN KHOI LUONG SUY RONG %%%%% % - KHAU -% rc01_nga=T01*rc1_nga; rc01=rc01_nga(1:3); Jt11=diff(rc01,q1); Jt12=diff(rc01,q2); Jt13=diff(rc01,q3); Jt14=diff(rc01,q4); Jt15=diff(rc01,q5); Jt16=diff(rc01,q6); Jt1=[Jt11 Jt12 Jt13 Jt14 Jt15 Jt16] A01=R01; c1=t*u1; c2=t*u2; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 84 LUẬN VĂN THẠC SĨ 09/2018 c3=t*u3; c4=t*u4; c5=t*u5; c6=t*u6; B01=(subs(A01,{q1,q2,q3,q4,q5,q6},{c1,c2,c3,c4,c5,c6})) D01=(diff(B01,t)) A01cham=simplify(subs(D01,{c1,c2,c3,c4,c5,c6,u1,u2,u3,u4,u5,u6}, {q1,q2,q3,q4,q5,q6,q1cham,q2cham,q3cham,q4cham,q5cham,q6cham})) omega_nga=simplify(A01cham*transpose(A01)) omega=transpose([omega_nga(3,2) omega_nga(1,3) omega_nga(2,1)]) Jr11=diff(omega,q1cham); Jr12=diff(omega,q2cham); Jr13=diff(omega,q3cham); Jr14=diff(omega,q4cham); Jr15=diff(omega,q5cham); Jr16=diff(omega,q6cham); Jr1=[Jr11 Jr12 Jr13 Jr14 Jr15 Jr16] I01=A01*I1*transpose(A01) Mq=simplify(m1*transpose(Jt1)*Jt1 + transpose(Jr1)*I01*Jr1) % - KHAU -% rc02_nga=T02*rc2_nga; rc02=rc02_nga(1:3); Jt21=diff(rc02,q1); Jt22=diff(rc02,q2); Jt23=diff(rc02,q3); Jt24=diff(rc02,q4); Jt25=diff(rc02,q5); Jt26=diff(rc02,q6); Jt2=[Jt21 Jt22 Jt23 Jt24 Jt25 Jt26] A02=R02; c1=t*u1; c2=t*u2; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 85 LUẬN VĂN THẠC SĨ 09/2018 c3=t*u3; c4=t*u4; c5=t*u5; c6=t*u6; B02=(subs(A02,{q1,q2,q3,q4,q5,q6},{c1,c2,c3,c4,c5,c6})) D02=(diff(B02,t)) A02cham=simplify(subs(D02,{c1,c2,c3,c4,c5,c6,u1,u2,u3,u4,u5,u6}, {q1,q2,q3,q4,q5,q6,q1cham,q2cham,q3cham,q4cham,q5cham,q6cham})) omega_nga=simplify(A02cham*transpose(A02)) omega=transpose([omega_nga(3,2) omega_nga(1,3) omega_nga(2,1)]) Jr21=diff(omega,q1cham); Jr22=diff(omega,q2cham); Jr23=diff(omega,q3cham); Jr24=diff(omega,q4cham); Jr25=diff(omega,q5cham); Jr26=diff(omega,q6cham); Jr2=[Jr21 Jr22 Jr23 Jr24 Jr25 Jr26] I02=A02*I2*transpose(A02) Mq=simplify(Mq + m2*transpose(Jt2)*Jt2 + transpose(Jr2)*I02*Jr2) % - KHAU -% rc03_nga=T03*rc3_nga; rc03=rc03_nga(1:3); Jt31=diff(rc03,q1); Jt32=diff(rc03,q2); Jt33=diff(rc03,q3); Jt34=diff(rc03,q4); Jt35=diff(rc03,q5); Jt36=diff(rc03,q6); Jt3=[Jt31 Jt32 Jt33 Jt34 Jt35 Jt36] A03=R03; c1=t*u1; c2=t*u2; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 86 LUẬN VĂN THẠC SĨ 09/2018 c3=t*u3; c4=t*u4; c5=t*u5; c6=t*u6; B03=(subs(A03,{q1,q2,q3,q4,q5,q6},{c1,c2,c3,c4,c5,c6})) D03=(diff(B03,t)) A03cham=simplify(subs(D03,{c1,c2,c3,c4,c5,c6,u1,u2,u3,u4,u5,u6}, {q1,q2,q3,q4,q5,q6,q1cham,q2cham,q3cham,q4cham,q5cham,q6cham})) omega_nga=simplify(A03cham*transpose(A03)) omega=transpose([omega_nga(3,2) omega_nga(1,3) omega_nga(2,1)]) Jr31=diff(omega,q1cham); Jr32=diff(omega,q2cham); Jr33=diff(omega,q3cham); Jr34=diff(omega,q4cham); Jr35=diff(omega,q5cham); Jr36=diff(omega,q6cham); Jr3=[Jr31 Jr32 Jr33 Jr34 Jr35 Jr36] I03=A03*I3*transpose(A03) Mq=simplify(Mq + m3*transpose(Jt3)*Jt3 + transpose(Jr3)*I03*Jr3) % - KHAU -% rc04_nga=T04*rc4_nga; rc04=rc04_nga(1:3); Jt41=diff(rc04,q1); Jt42=diff(rc04,q2); Jt43=diff(rc04,q3); Jt44=diff(rc04,q4); Jt45=diff(rc04,q5); Jt46=diff(rc04,q6); Jt4=[Jt41 Jt42 Jt43 Jt44 Jt45 Jt46] A04=R04; c1=t*u1; c2=t*u2; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 87 LUẬN VĂN THẠC SĨ 09/2018 c3=t*u3; c4=t*u4; c5=t*u5; c6=t*u6; B04=(subs(A04,{q1,q2,q3,q4,q5,q6},{c1,c2,c3,c4,c5,c6})) D04=(diff(B04,t)) A04cham=simplify(subs(D04,{c1,c2,c3,c4,c5,c6,u1,u2,u3,u4,u5,u6}, {q1,q2,q3,q4,q5,q6,q1cham,q2cham,q3cham,q4cham,q5cham,q6cham})) omega_nga=simplify(A04cham*transpose(A04)) omega=transpose([omega_nga(3,2) omega_nga(1,3) omega_nga(2,1)]) Jr41=diff(omega,q1cham); Jr42=diff(omega,q2cham); Jr43=diff(omega,q3cham); Jr44=diff(omega,q4cham); Jr45=diff(omega,q5cham); Jr46=diff(omega,q6cham); Jr4=[Jr41 Jr42 Jr43 Jr44 Jr45 Jr46] I04=A04*I4*transpose(A04) Mq=simplify(Mq + m4*transpose(Jt4)*Jt4 + transpose(Jr4)*I04*Jr4) % - KHAU -% rc05_nga=T05*rc5_nga; rc05=rc05_nga(1:3); Jt51=diff(rc05,q1); Jt52=diff(rc05,q2); Jt53=diff(rc05,q3); Jt54=diff(rc05,q4); Jt55=diff(rc05,q5); Jt56=diff(rc05,q6); Jt5=[Jt51 Jt52 Jt53 Jt54 Jt55 Jt56] A05=R05; c1=t*u1; c2=t*u2; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 88 LUẬN VĂN THẠC SĨ 09/2018 c3=t*u3; c4=t*u4; c5=t*u5; c6=t*u6; B05=(subs(A05,{q1,q2,q3,q4,q5,q6},{c1,c2,c3,c4,c5,c6})) D05=(diff(B05,t)) A05cham=simplify(subs(D05,{c1,c2,c3,c4,c5,c6,u1,u2,u3,u4,u5,u6}, {q1,q2,q3,q4,q5,q6,q1cham,q2cham,q3cham,q4cham,q5cham,q6cham})) omega_nga=simplify(A05cham*transpose(A05)) omega=transpose([omega_nga(3,2) omega_nga(1,3) omega_nga(2,1)]) Jr51=diff(omega,q1cham); Jr52=diff(omega,q2cham); Jr53=diff(omega,q3cham); Jr54=diff(omega,q4cham); Jr55=diff(omega,q5cham); Jr56=diff(omega,q6cham); Jr5=[Jr51 Jr52 Jr53 Jr54 Jr55 Jr56] I05=A05*I5*transpose(A05) Mq=simplify(Mq + m5*transpose(Jt5)*Jt5 + transpose(Jr5)*I05*Jr5) % - KHAU -% rc06_nga=T06*rc6_nga; rc06=rc06_nga(1:3); Jt61=diff(rc06,q1); Jt62=diff(rc06,q2); Jt63=diff(rc06,q3); Jt64=diff(rc06,q4); Jt65=diff(rc06,q5); Jt66=diff(rc06,q6); Jt6=[Jt61 Jt62 Jt63 Jt64 Jt65 Jt66] A06=R06; c1=t*u1; LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 89 LUẬN VĂN THẠC SĨ 09/2018 c2=t*u2; c3=t*u3; c4=t*u4; c5=t*u5; c6=t*u6; B06=(subs(A06,{q1,q2,q3,q4,q5,q6},{c1,c2,c3,c4,c5,c6})) D06=(diff(B06,t)) A06cham=simplify(subs(D06,{c1,c2,c3,c4,c5,c6,u1,u2,u3,u4,u5,u6}, {q1,q2,q3,q4,q5,q6,q1cham,q2cham,q3cham,q4cham,q5cham,q6cham})) omega_nga=simplify(A06cham*transpose(A06)) omega=transpose([omega_nga(3,2) omega_nga(1,3) omega_nga(2,1)]) Jr61=diff(omega,q1cham); Jr62=diff(omega,q2cham); Jr63=diff(omega,q3cham); Jr64=diff(omega,q4cham); Jr65=diff(omega,q5cham); Jr66=diff(omega,q6cham); Jr6=[Jr61 Jr62 Jr63 Jr64 Jr65 Jr66] I06=A06*I6*transpose(A06) Mq=simplify(Mq + m6*transpose(Jt6)*Jt6 + transpose(Jr6)*I06*Jr6) %%%%% TINH THE NANG CUA ROBOT %%%%% g0=transpose([0 -g]) TN=-m1*transpose(g0)*rc01 TN=TN-m2*transpose(g0)*rc02 TN=TN-m3*transpose(g0)*rc03 TN=TN-m4*transpose(g0)*rc04 TN=TN-m5*transpose(g0)*rc05 TN=TN-m6*transpose(g0)*rc06 TN1=diff(TN,q1) TN2=diff(TN,q2) TN3=diff(TN,q3) TN4=diff(TN,q4) TN5=diff(TN,q5) LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 90 LUẬN VĂN THẠC SĨ 09/2018 TN6=diff(TN,q6) The_nang=transpose([TN1 TN2 TN3 TN4 TN5 TN6]) %%%%% TINH MA TRAN LY TAM CORIOLIS %%%%% Mq1=Mq(1:6,1); Mq2=Mq(1:6,2); Mq3=Mq(1:6,3); Mq4=Mq(1:6,4); Mq5=Mq(1:6,5); Mq6=Mq(1:6,6); d_Mq_d_q=[diff(Mq1,q1) diff(Mq1,q2) diff(Mq1,q3) diff(Mq1,q4) diff(Mq1,q5) diff(Mq1,q6) diff(Mq2,q1) diff(Mq2,q2) diff(Mq2,q3) diff(Mq2,q4) diff(Mq2,q5) diff(Mq2,q6) diff(Mq3,q1) diff(Mq3,q2) diff(Mq3,q3) diff(Mq3,q4) diff(Mq3,q5) diff(Mq3,q6) diff(Mq4,q1) diff(Mq4,q2) diff(Mq4,q3) diff(Mq4,q4) diff(Mq4,q5) diff(Mq4,q6) diff(Mq5,q1) diff(Mq5,q2) diff(Mq5,q3) diff(Mq5,q4) diff(Mq5,q5) diff(Mq5,q6) diff(Mq6,q1) diff(Mq6,q2) diff(Mq6,q3) diff(Mq6,q4) diff(Mq6,q5) diff(Mq6,q6)] qcham=transpose([q1cham q2cham q3cham q4cham q5cham q6cham]) In=[1 0 0 010000 001000 000100 000010 0 0 1] Matran_C=d_Mq_d_q*kron(In,qcham)-1/2*transpose(d_Mq_d_q*kron(qcham,In)) %%%%% PHUONG TRINH TINH MOMEN TAI CAC KHOP %%%%% syms q1chamcham q2chamcham q3chamcham q4chamcham q5chamcham q6chamcham qcham=transpose([q1cham q2cham q3cham q4cham q5cham q6cham]) qchamcham=transpose([q1chamcham q2chamcham q3chamcham q4chamcham q5chamcham q6chamcham]) Momen=Mq*qchamcham + Matran_C*qcham + The_nang LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 91 LUẬN VĂN THẠC SĨ 09/2018 for cs=1:1:(N+1) temp_momen=subs(Momen,{q1,q2,q3,q4,q5,q6,q1cham,q2cham,q3cham,q4cham,q5cham,q6cham, q1chamcham,q2chamcham,q3chamcham,q4chamcham,q5chamcham,q6chamcham,}, {kq_dhn_q(cs,1),kq_dhn_q(cs,2),kq_dhn_q(cs,3),kq_dhn_q(cs,4),kq_dhn_q(cs,5),kq_dhn_q(cs,6), kq_dhn_qcham(cs,1),kq_dhn_qcham(cs,2),kq_dhn_qcham(cs,3),kq_dhn_qcham(cs,4),kq_dhn_qcham(cs,5),k q_dhn_qcham(cs,6), kq_dhn_qchamcham(cs,1),kq_dhn_qchamcham(cs,2),kq_dhn_qchamcham(cs,3),kq_dhn_qchamcham(cs,4),k q_dhn_qchamcham(cs,5),kq_dhn_qchamcham(cs,6),}) kq_dlhn_momen(cs,:)=transpose(temp_momen) end save kq_dlhn_momen kq_dlhn_momen save kq_dlhn_momen.dat kq_dlhn_momen /ascii disp('COMPLETE!!!') LƯU QUỐC KHÁNH – CB150070 – CĐT.KH.2015B Trang 92 ... nghệ hàn C8-108 Sau trình tìm hiểu robot DWE-V1, với định hướng PGS.TS Bùi Văn Hạnh, em nhận đề tài Luận văn : “ Điều khiển robot hàn hồ quang, đồ gá thiết bị di trượt ” LƯU QUỐC KHÁNH – CB150070... điều khiển Phần mềm tính tốn điều khiển đóng vai trò liên kết người sử dụng điều khiển robot để mô phỏng, điều khiển, giám sát robot Phần mềm có chức thực thiết kế quỹ đạo, mô điều khiển robot. .. lực học điều khiển Robot - Không gian làm việc rộng - Độ linh hoạt hướng tốt thao tác nhiều ứng dụng khác 1.4.3 Cấu tạo hệ thống điều khiển cho robot hàn DWE-V1 Hệ thống điều khiển robot hàn DWE-V1