báo cáo Robot ABB IRB 4400

Robot ABB-IRB 4400 GVHD: Ts.Phạm Công Bằng Mục lục I. Tổng Quan Về Robot ABB-IRB 4400…………………………………………………… II. Thông Số Động Học Của Robot ABB-IRB 4400……………………………………….. 1. Xác định hệ tọa độ cho các khâu……………………………………………………… 2. Xác định bảng thông số D-H………………………………………………………….. 3. Giải bài toán động học thuận…………………………………………………………. 4. Giải bài toán động học nghịch…………………………………………………………. 5. Không gian làm việc…………………………………………………………………… 6. Ma trận Jacobian…………………………………………………………………….. III. Mô phỏng chuyển động…………………………………………………………… Nhóm 4W 1 Robot ABB-IRB 4400 GVHD: Ts.Phạm Công Bằng II, Thông Số Động Học Của Robot ABB-IRB 4400 1. Xác định hệ tọa độ cho các khâu: Nhóm 4W 2 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 Hình. Hệ tọa độ các khâu 2. Xác định bảng thông số D-H: Nhóm 4W 3 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 Bảng thông số D-H: 𝑖 𝑎𝑖 𝛼𝑖 𝑑𝑖 𝜃𝑖 1 400 −900 860 𝜃1 2 890 0 0 𝜃2 3 930 0 0 𝜃3 3. Giải bài toán động học thuận: - Bài toán động học thuận: 𝑐𝑜𝑠𝜃1 𝑠𝑖𝑛𝜃1 0 1𝑇 = [ 0 0 0 −𝑠𝑖𝑛𝜃1 0 𝑐𝑜𝑠𝜃1 −1 0 0 0 400𝑐𝑜𝑠𝜃1 400𝑠𝑖𝑛𝜃1 ] 860 1 𝑐𝑜𝑠𝜃2 𝑠𝑖𝑛𝜃 2 1 2𝑇 = [ 0 0 −𝑠𝑖𝑛𝜃2 𝑐𝑜𝑠𝜃2 0 0 0 0 1 0 890𝑐𝑜𝑠𝜃2 890𝑠𝑖𝑛𝜃2 ] 0 1 𝑐𝑜𝑠𝜃3 𝑠𝑖𝑛𝜃3 2 3𝑇 = [ 0 0 −𝑠𝑖𝑛𝜃3 𝑐𝑜𝑠𝜃3 0 0 0 0 1 0 930𝑐𝑜𝑠𝜃3 930𝑠𝑖𝑛𝜃3 ] 0 1 Đặt: 𝑠𝑖𝑛𝜃1 = 𝑠1 ; 𝑐𝑜𝑠𝜃1 = 𝑐1 ; 𝑠𝑖𝑛𝜃2 = 𝑠2 ; 𝑐𝑜𝑠𝜃2 = 𝑐2 . 𝑠𝑖𝑛𝜃3 = 𝑠3 ; 𝑐𝑜𝑠𝜃3 = 𝑐3 ; sin(𝜃2 + 𝜃3 ) = 𝑠23 ; cos(𝜃2 + 𝜃3 ) = 𝑐23 . 𝑐1 𝑐23 𝑠1 𝑐23 0 3𝑇 = [ −𝑠23 0 −𝑐1 𝑠23 −𝑠1 𝑠23 −𝑐23 0 −𝑠1 𝑐1 0 0 400𝑐1 + 890𝑐1 𝑐2 + 930𝑐1 𝑐23 400𝑠1 + 890𝑠1 𝑐2 + 930𝑠1 𝑐23 ] 860 − 890𝑠2 − 930𝑠23 1 - Tọa độ điểm cuối: 400𝑐1 + 890𝑐1 𝑐2 + 930𝑐1 𝑐23 𝑥 [𝑦] = [400𝑠1 + 890𝑠1 𝑐2 + 930𝑠1 𝑐23 ] 𝑧 860 − 890𝑠2 − 930𝑠23 Nhóm 4W 4 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 - Trường hợp cụ thể: 𝜃1 = 0; 𝜃2 = −900 ; 𝜃3 = 900 Khi đó:𝑥 = 1330(𝑚𝑚); 𝑦 = 0(𝑚𝑚); 𝑧 =1750(𝑚𝑚) Hình. Vị trí của robot 4. Giải bài toán động học nghịch: - Bài toán động học nghịch: + Tính góc 𝜃1 : 𝜃1 = 𝑎𝑡𝑎𝑛2𝑑(𝑦, 𝑥); Nhóm 4W 5 Robot ABB-IRB 4400 GVHD: Ts.Phạm Công Bằng Áp dụng định luật Cosine trong tam giác, tính 𝜃2 và 𝜃3 : + Tính góc 𝜃3 cos 𝜃3 = 2 𝑥𝐶 +𝑧𝐶2 −8902 −9302 2∗890∗930 sin 𝜃3 = ±√1 − cos 𝜃3 2 𝜃3 = atan2d(sin 𝜃3 , cos 𝜃3 ) + Tính góc 𝜃2 cosψ = 𝑥𝐶2 + 𝑧𝐶2 + 8902 − 9302 2 ∗ 890 ∗ √𝑥𝐶2 + 𝑧𝐶2 sin ψ = ±√1 − cos ψ2 ψ = atan2d(sin ψ , cos ψ) 𝜃1 = β ± ψ Trong đó: 𝑥 𝑥𝐶 = cos(𝜃 ) − 400; 𝑧𝐶 = 𝑧 − 860; β = atan2d(𝑧𝐶 , 𝑥𝐶 ). 1 Nhóm 4W 6 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 - Trường hợp cụ thể: 𝑥 = 1290(𝑚𝑚); 𝑦 = 0(𝑚𝑚); 𝑧 = −70(𝑚𝑚) Khi đó: 𝜃1 = 0; 𝜃2 = 0; 𝜃3 = 900 Hình. Vị trí của robot 5. Không gian làm việc: Nhóm 4W 7 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 Hình. Không gian làm việc 6. Ma trận Jacobian: −400𝑠1 − 890𝑠1 𝑐2 − 930𝑠1 𝑐23 𝐽(𝜃) = [ 400𝑐1 + 890𝑐1 𝑐2 + 930𝑐1 𝑐23 0 0 −890𝑐1 𝑠2 − 930𝑐1 𝑠23 −890𝑠1 𝑠2 − 930𝑠1 𝑠23 −890𝑐2 − 930𝑐23 −930𝑐1 𝑠23 −930𝑠1 𝑠23 ] −930𝑐23 - Tính định thức ma trận Jacobian det( 0𝐽(𝜃)) = 890 ∗ 930𝑠3 (860 + 890𝑐2 + 930𝑐23 ) - Xác định vùng kì dị khi det( 0𝐽(𝜃)) = 0 ⇔[ 𝑠𝑖𝑛𝜃3 = 0 860 + 890𝑐𝑜𝑠𝜃2 + 930 cos(𝜃2 + 𝜃3 ) = 0 + 𝑠𝑖𝑛𝜃3 = 0 ⇔ 𝜃3 = 𝑘𝜋. Nhóm 4W 8 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 Khi 𝜃3 = 00 là vùng kì dị det( 0𝐽(𝜃)) = 𝑙2 𝑙3 𝑠3 (𝑙1 + 𝑙2 𝑐2 + 𝑙3 𝑐23 ) + 860 + 890𝑐𝑜𝑠𝜃2 + 930 cos(𝜃2 + 𝜃3 ) = 0 III, Mô phỏng chuyển động: - Đặt gốc tọa độ và thiết lập vị trí ban đầu trong solidworks Hình. Hệ tọa độ robot trong solidworks - Đưa mô hình vào matlab, tạo các sơ đồ khối trong cửa sổ Simulink: Nhóm 4W 9 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 Hình. Sơ đồ khối trong cửa sổ Simulink matlab Chỉnh sửa thông số mô hình: + Kiểm tra, hiệu chỉnh liên kết xoay/trượt . + Kiểm tra, hiệu chỉnh các vector vị trí của hệ tọa độ trọng tâm, hệ tọa độ gốc và hệ tọa độ liên kết của tất cả các Part. + Kiểm tra, hiệu chỉnh các ma trận định hướng của hệ tọa độ trọng tâm, hệ tọa độ gốc và hệ tọa độ liên kết của tất cả các Part. Nhóm 4W 10 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 Hình. Hệ tọa độ robot trong Simulink matlab - Lập trình điều khiển: (Phụ lục 1) Lập trình để tạo ra biến vt (vt bao gồm thời gian, góc 𝜃1 , 𝜃2 , 𝜃3 ). Robot sẽ di chuyển theo một quỹ đạo đã lập trình. Cách lập trình: + Đầu tiên phải xác định tọa độ chữ cần vẽ [x,y,z] theo thời gian lấy mẫu là 0.01s ( đưởng thẳng vẽ ra sẽ mịn hơn). + Tiếp theo ta đưa tọa độ đã xác định ở trên vào bài toán động học nghịch để giải ra các góc 𝜃1 , 𝜃2 , 𝜃3 theo thời gian. Lưu số giải được vào biến vt để thực hiện mô phỏng. Nhóm 4W 11 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 Hình. Kết quả mô phỏng Nhóm 4W 12 GVHD: Ts.Phạm Công Bằng Robot ABB-IRB 4400 PHỤ LỤC 1 Code biến vt: Thời gian để vẽ là 8s, 2s cho mõi chữ. function [vt]=TEN() clc; clear all; t=[0:0.01:8]; a1=400; a2=890; a3=930; a4=860; mt=zeros(); vt=zeros(); %CHU S %Hoach dinh quy dao chuyen dong------------------------------------------z=860+890; for i=1:1:101 yp = 900; xp = -350; R = 100; phi = 225; x = xp+R*cosd(phi*t(i)+45); y = yp+R*sind(phi*t(i)+45); mt(i,1)=x; mt(i,2)=y; end for i=101:1:201 yp = 700; xp = -350; R = 100; x = xp-R*cosd(phi*(t(i)-1)+90); y = yp+R*sind(phi*(t(i)-1)+90); mt(i,1)=x; mt(i,2)=y; end phi = 225; %CHU P %Hoach dinh quy dao chuyen dong------------------------------------------for i=201:1:301 y = 600; yp=400; y=y+yp*(t(i)-2); mt(i,1)=-150; Nhóm 4W 13 Robot ABB-IRB 4400 GVHD: Ts.Phạm Công Bằng mt(i,2)=y; end for i=301:1:401 yp = 900; xp = -150; R = 100; phi = 180; x = xp-R*cosd(phi*(t(i)-3)+90); y = yp+R*sind(phi*(t(i)-3)+90); mt(i,1)=x; mt(i,2)=y; end %CHU P %Hoach dinh quy dao chuyen dong------------------------------------------for i=401:1:501 y = 600; yp=400; y=y+yp*(t(i)-4); mt(i,1)=50; mt(i,2)=y; end for i=501:1:601 yp = 900; xp = 50; R = 100; phi = 180; x = xp-R*cosd(phi*(t(i)-5)+90); y = yp+R*sind(phi*(t(i)-5)+90); mt(i,1)=x; mt(i,2)=y; end %Chu Q %Hoach dinh quy dao chuyen dong------------------------------------------for i=601:1:701 yp = 800; xp = 450; R = 200; phi = 360; x = xp+R*cosd(phi*(t(i)-6)-45); y = yp+R*sind(phi*(t(i)-6)-45); mt(i,1)=x; mt(i,2)=y; end for i=701:1:801 yp = 750; xp = 450; hs=sqrt((800-yp)^2+100^2); dd=200; y = 800-(hs+dd*(t(i)-7))*cosd(45); x = xp+(hs+dd*(t(i)-7))*sind(45); mt(i,1)=x; mt(i,2)=y; end % Bai toan dong hoc nghich for j=1:1:801 Nhóm 4W 14 Robot ABB-IRB 4400 GVHD: Ts.Phạm Công Bằng %Xac dinh theta 1--------------------------------------------------------d1 = atan2d(mt(j,2),mt(j,1)); vt(j,2)=d1; %Xac dinh theta 3--------------------------------------------------------if (fix(d1)==90||fix(d1)==-90) d1=0; mt(j,1)=mt(j,2); else d1=d1; end xt = mt(j,1)*cosd(d1)*(1+tand(d1)^2); c3r = ((xt-a1)^2+(z-a4)^2-a2^2-a3^2)/(2*a2*a3); s3r = sqrt(1-c3r^2); d3r = atan2d(s3r,c3r); d3 = d3r-90; %Xac dinh theta 2--------------------------------------------------------ACb = (xt-a1)^2+(z-a4)^2; AC = sqrt(ACb); c2r = (a2^2+ACb-a3^2)/(2*a2*AC); s2r = sqrt(1-c2r^2); beta = atan2d(s2r,c2r); d2r = beta+atan2d(z-a4,xt-a1); d2 = 90-d2r; vt(j,3)=d2; vt(j,4)=d3; end vt(1:801,1)=t; Nhóm 4W 15 [...]... nghịch để giải ra các góc 𝜃1 , 𝜃2 , 𝜃3 theo thời gian Lưu số giải được vào biến vt để thực hiện mô phỏng Nhóm 4W 11 GVHD: Ts.Phạm Công Bằng Robot ABB- IRB 4400 Hình Kết quả mô phỏng Nhóm 4W 12 GVHD: Ts.Phạm Công Bằng Robot ABB- IRB 4400 PHỤ LỤC 1 Code biến vt: Thời gian để vẽ là 8s, 2s cho mõi chữ function [vt]=TEN() clc; clear all; t=[0:0.01:8]; a1=400; a2=890; a3=930; a4=860;...GVHD: Ts.Phạm Công Bằng Robot ABB- IRB 4400 Hình Hệ tọa độ robot trong Simulink matlab - Lập trình điều khiển: (Phụ lục 1) Lập trình để tạo ra biến vt (vt bao gồm thời gian, góc 𝜃1 , 𝜃2 , 𝜃3 ) Robot sẽ di chuyển theo một quỹ đạo đã lập trình Cách lập trình: + Đầu tiên phải xác định tọa... yp+R*sind(phi*(t(i)-1)+90); mt(i,1)=x; mt(i,2)=y; end phi = 225; %CHU P %Hoach dinh quy dao chuyen dong for i=201:1:301 y = 600; yp=400; y=y+yp*(t(i)-2); mt(i,1)=-150; Nhóm 4W 13 Robot ABB- IRB 4400 GVHD: Ts.Phạm Công Bằng mt(i,2)=y; end for i=301:1:401 yp = 900; xp = -150; R = 100; phi = 180; x = xp-R*cosd(phi*(t(i)-3)+90); y = yp+R*sind(phi*(t(i)-3)+90); mt(i,1)=x; mt(i,2)=y; end %CHU... xp = 450; hs=sqrt((800-yp)^2+100^2); dd=200; y = 800-(hs+dd*(t(i)-7))*cosd(45); x = xp+(hs+dd*(t(i)-7))*sind(45); mt(i,1)=x; mt(i,2)=y; end % Bai toan dong hoc nghich for j=1:1:801 Nhóm 4W 14 Robot ABB- IRB 4400 GVHD: Ts.Phạm Công Bằng %Xac dinh theta 1 d1 = atan2d(mt(j,2),mt(j,1)); vt(j,2)=d1; %Xac dinh theta 3 if (fix(d1)==90||fix(d1)==-90) .. .Robot ABB- IRB 4400 GVHD: Ts.Phạm Công Bằng II, Thông Số Động Học Của Robot ABB- IRB 4400 Xác định hệ tọa độ cho các khâu: Nhóm 4W GVHD: Ts.Phạm Công Bằng Robot ABB- IRB 4400. .. hiện mô phỏng Nhóm 4W 11 GVHD: Ts.Phạm Công Bằng Robot ABB- IRB 4400 Hình Kết mô phỏng Nhóm 4W 12 GVHD: Ts.Phạm Công Bằng Robot ABB- IRB 4400 PHỤ LỤC Code biến vt: Thời gian để vẽ là... Công Bằng Robot ABB- IRB 4400 Hình Hệ tọa độ robot Simulink matlab - Lập trình điều khiển: (Phụ lục 1) Lập trình để tạo biến vt (vt bao gồm thời gian, góc