Đang tải... (xem toàn văn)
LỜI MỞ ĐẦU Ngày nay, với sự phát triển của khoa học kĩ thuật, robot ngày càng trở nên phổ biến, có ứng dụng rộng rãi trong nhiều ngành. Robot có thể giúp con người làm việc trong những môi trường khắc nghiệt, hoặc tăng năng suất, hiệu quả lao động trong công nghiệp. Như vậy, nghiên cứu và triển khai robot trong thực tế có ý nghĩa thiết thực trong sự phát triển của xã hội. Qua bài tập lớn robot EP4000D của hãng Yaskawa, chúng em đã hiểu thêm về ứng dụng của robot trong thực tế, cũng như ôn luyện bài tập qua thông số của robot. Robot EP4000D có thiết kế nhỏ gọn, cho phép robot làm việc trong vùng hạn chế. Hơn nữa, robot có kết cấu 6 trục quay, với cơ cấu chấp hành gồm khí nén và động cơ điện giúp robot chuyển động linh hoạt, phù hợp với nhiều ứng dụng thực tế như gắp, hàn, sơn,... Bản báo cáo được chia thành sáu phần như sau: Giới thiệu robot EP4000D. Tính động học thuận vị trí. Tính ma trận Jacobi. Tính động học ngược vị trí. Thiết kế quỹ đạo khớp dạng bậc 3. Mô phỏng động lực học robot
Mục lục LỜI MỞ ĐẦU NỘI DUNG Giới thiệu tổng quan Robot EP4000D Robot EP4000D phát triển để ứng dụng vào dây truyền sản xuất công nghiệp với phù hợp với nhiều công việc khác như: Tính tốn động học thuận vị trí 2.1 Xác định hệ trục tọa độ 2.2 Lập bảng thông số Denavit – Hartenberg 2.3 Thiết lập hệ phương trình động học cho robot EP4000D: 2.4 Chương trình Matlab để nhập liệu, hiển thị kết 10 Tính tốn ma trận Jacoby 11 i 3.1 Xác định ma trận Tn 11 3.2 Xác định ma trận 3.3 Xác định ma trận Jacoby 12 3.4 Chương trình Matlab tính tốn ma trận Jacoby 12 H J 11 Tính tốn động học đảo vị trí Robot 13 4.1 Tính ma trận nghịch đảo ma trận vị trí khớp 13 4.2 Tính tốn động học ngược robot 14 Thiết kế quỹ đạo chuyển động cho khớp Robot theo quỹ đạo dạng bậc 15 Mô phỏng động lực học Robot EP4000D 17 a Thiết kế Robot 17 b Thiết kế điều khiển 20 DANH MỤC TÀI LIỆU THAM KHẢO 23 LỜI MỞ ĐẦU Ngày nay, với phát triển khoa học kĩ thuật, robot ngày trở nên phổ biến, có ứng dụng rộng rãi nhiều ngành Robot giúp người làm việc môi trường khắc nghiệt, tăng suất, hiệu lao động công nghiệp Như vậy, nghiên cứu triển khai robot thực tế có ý nghĩa thiết thực phát triển xã hội Qua tập lớn robot EP4000D hãng Yaskawa, chúng em hiểu thêm ứng dụng robot thực tế, ôn luyện tập qua thông số robot Robot EP4000D có thiết kế nhỏ gọn, cho phép robot làm việc vùng hạn chế Hơn nữa, robot có kết cấu trục quay, với cấu chấp hành gồm khí nén động điện giúp robot chuyển động linh hoạt, phù hợp với nhiều ứng dụng thực tế gắp, hàn, sơn, Bản báo cáo chia thành sáu phần sau: - Giới thiệu robot EP4000D - Tính động học thuận vị trí - Tính ma trận Jacobi - Tính động học ngược vị trí - Thiết kế quỹ đạo khớp dạng bậc - Mô động lực học robot Nhóm chúng em gồm sinh viên CTTN-ĐKTĐ-K62: + Bùi Đăng Bảo - 20173662 + Đàm Quốc Huy - 20173168 + Mẫn Bá Hữu - 20170140 + Nguyễn Văn Tiệp – 20175803 NỘI DUNG Giới thiệu tổng quan Robot EP4000D Robot EP4000D phát triển để ứng dụng vào dây truyền sản xuất công nghiệp với phù hợp với nhiều công việc khác như: • Robot gắp (Handling) • Robot nâng bốc, đóng gói ( Picking/packing, palletizing) • Robot hàn, hàn điểm ( Arc handling, spot welding) • Robot sơn ( Painting) • Robot lắp ráp ( Assembly/distributing) Hình 1.1 Robot EP4000D Do việc sử dụng sản phẩm robot EP4000D Yaskawa đảm bảo tính tương thích, độ quán chất lượng dịch vụ cao việc tự động hóa máy móc, dây chuyền sản xuất Yaskawa cung cấp tát loại robot để giải toán đáp ứng yêu cầu khác khách hàng Các robot EP4000D có trọng tải 200 kg, chiều ngang 3,505 mm tầm với thẳng đứng 2,629 mm Thiết kế gọn nhẹ robot cho phép làm việc không gian chật hẹp cách linh hoạt Đây loại robot có sáu trục quay vận hành khí nén kết hợp với động điện, giúp robot vận hành cách linh hoạt, di truyền dễ dàng dến vị trí đảm bảo độ xác robot cơng việc Hình ảnh bên cho ta thấy kích thước robot, vùng làm việc robot vùng có màu xanh Hình 1.2 Vùng làm việc Robot Hình ảnh bên mơ tả thơng số kĩ thuật robot: Hình 1.3 Thơng số kĩ thuật robot Hình ảnh mơ tả trục quay robot: Hình 1.4 Tên phận trục làm việc Tính tốn động học thuận vị trí 2.1 Xác định hệ trục tọa độ Các hệ trục tọa độ chọn hình vẽ theo quy tắc Denavit – Hartenberg O0 Z0 X0 O1 O3 O2 Z1 X0 X1 X2, X3 Z2 Z3 O4 , O5 X4, X5 Z4 Z5 O6 X6 Z6 Hình 2.1 Thiết lập hệ trục tọa độ Denavit – Hartenberg 2.2 Lập bảng thông số Denavit – Hartenberg Khâu i i 1 2 3 4 5 6 −90 550 di 180 1100 90 250 −90 2135 90 0 0 255 - Trong đó: 𝜃𝑖 góc quay quanh trục 𝑧𝑖−1 𝑥𝑖−1 𝑥𝑖 𝛼𝑖 𝑔ó𝑐 𝑞𝑢𝑎𝑦 𝑞𝑢𝑎𝑛ℎ 𝑡𝑟ụ𝑐 𝑥𝑖 𝑔𝑖ữ𝑎 𝑧𝑖−1 𝑣à 𝑧𝑖 𝑎𝑖 𝑙à 𝑘ℎ𝑜ả𝑛𝑔 𝑐á𝑐ℎ 𝑔𝑖ữ𝑎 𝑘ℎớ𝑝 𝑙𝑖ê𝑛 𝑡𝑖ế𝑝 𝑡ℎ𝑒𝑜 𝑝ℎươ𝑛𝑔 𝑧𝑖 𝑑𝑖 khoảng cách khớp liên phương 𝑧𝑖−1 2.3 Thiết lập hệ phương trình động học cho robot EP4000D: a Các ma trận đổi toạ độ: cos 1 sin A1 = − sin 1 550 cos 1 cos 1 550sin 1 −1 0 0 cos 3 sin A3 = 0 sin 3 − cos 3 0 250 cos 3 250sin 3 cos sin A2 = cos sin A4 = sin 1100 cos 1100sin −1 − cos 0 − sin cos −1 0 0 2135 cos 5 sin A5 = 0 sin 5 − cos 5 0 cos sin A6 = 0 0 1 − sin cos 0 0 255 Ma trận biến đổi nhất: nx n T6 = A1 A2 A3 A4 A5 A6 = y nz 0 ox ax oy ay oz az 0 px p y pz 1 Trong đó: nx = s6 (c4 s1 − s4 (c1s2 s3 + c1c2c3 )) + c6 (c5 (s1s4 + c4 (c1s2 s3 + c1c2c3 )) − s5 (c1c2 s3 − c1s2c3 )) ny = −s6 (c1c4 + s4 (s1s2 s3 + s1c2c3 )) − c6 (c5 (c1s4 − c4 (s1s2s3 + s1c2c3 )) + s5 (s1c2s3 − s1s2c3 )) nz = c6 (s5 (c2c3 + s2 s3 ) + c4c5 (c2 s3 − s2c3 )) − s4 s6 (c2 s3 − s2c3 ) ox = c6 (s1c4 − s4 (c1s2 s3 + c1c2c3 )) − s6 (c5 (s1s4 + c4 (c1s2 s3 + c1c2c3 )) − s5 (c1c2 s3 − c1s2c3 )) oy = s6 (c5 (c1s4 − c4 (s1s2 s3 + s1c2c3 )) + s5 (s1c2 s3 − s1s2c3 )) − c6 (c1c4 + s4 (s1s2 s3 + s1c2c3 )) oz = −s6 (s5 (c2c3 + s2 s3 ) + c4c5 (c2 s3 − s2c3 )) − c6 s4 (c2 s3 − s2c3 ) ax = s5 (s1s4 + c4 (c 1s2 s3 + c1c2c3 )) + c5 (c 1c2 s3 − c1s2c3 ) ay = c5 (s1c2 s3 − s1s2c3 ) − s5 (c1s4 − c4 (s1s2 s3 + s1c2c3 )) az = c4 s5 (c s3 − s2c3 ) − c5 (c2c3 + s2 s3 ) px = 550c1 + 1100c1c2 + 255s5 ( s1s4 + c4 (c1s2 s3 + c1c2c3 )) + 255c5 (c1c2 s3 − c1s2c3 ) + 250c1s2 s3 + 250c1c2c3 +2135c1s2 s3 − 2135c1s2c3 p y = 550 s1 + 1100 s1c2 − 255s5 (c1s4 − c4 ( s1s2 s3 + s1c2 c3 )) + 255c5 ( s1c2 s3 − s1s2 c3 ) + 2135s1c2 s3 − 2135s1s2 c3 +250 s1s2 s3 + 250 s1c2 c3 pz = 250c2 s3 − 2135c2c3 − 1100s2 − 250s2c3 − 2135s2 s3 − 255c4 (c2c3 + s2 s3 ) + 255c4 s5 (c2 s3 − s2c3 ) b Chương trình tính tổng qt >> syms a1 a2 a3 d4 d6; >> syms i1 i2 i3 i4 i5 i6; >> c1=cos(i1);c2=cos(i2);c3=cos(i3);c4=cos(i4); c5=cos(i5);c6=cos(i6); >> s1=sin(i1);s2=sin(i2);s3=sin(i3);s4=sin(i4); s5=sin(i5);s6=sin(i6); >> A1 = [c1 -s1 a1*c1; s1 c1 a1*s1; -1 0 ;0 0 1]; >> A2=[c2 s2 a2*s2; s2 -c2 a2*s2; 0 -1 ;0 0 1]; >> A3=[c3 s3 a3*c3; s3 -c3 a3*s3; 0; 0 1]; >> A4=[c4 -s4 0; s4 c4 0; -1 d4 0; 0 1]; >> A5=[c5 s5 0; s5 -c5 0; 0; 0 1]; >>A6=[c6 -s6 0; s6 c6 0; 0 0;0 d6 1]; c Xác định ma trận T theo hệ tọa độ từ khâu cuối >> T56 = A6; >> T46=A5*A6 ; >>T36 =A4*T46 ; >>T26=A3*T36 ; >>T16=A2*T26 ; >>T06=A1*T16 ; 2.4 Chương trình Matlab để nhập liệu, hiển thị kết % Tính ma ma trận Ai từ tính ma trận T th = sym('th',[1 6]); a = [550 1100 250 0 0]; d = [0 0 2135 255]; alpha = [-pi/2 pi pi/2 -pi/2 pi/2 0]; x = 1; A = cell(6,1); T = eye(4); for i = 1:6 A{i} = [cos(th(i)) round(-cos(alpha(i)),2)*sin(th(i)) round(sin(alpha(i)),2)*sin(th(i)) a(i)*cos(th(i)); sin(th(i)) round(cos(alpha(i)),2)*cos(th(i)) round(sin(alpha(i)),2)*cos(th(i)) a(i)*sin(th(i)); round(sin(alpha(i)),2) round(cos(alpha(i)),2) d(i); 0 1]; 10 T = T * A{i}; end fprintf('M\x61 tr\x1EADn T: \n'); disp(T); f(th) = T; % Nhập giá trị biến khớp, trả ma trận T fprintf('T\xEDnh m\x61 tr\x1EADn \x62i\x1EBFn \x64\x1ED5i T th\x65o \x62i\x1EBFn kh\x1EDBp nh\x1EADp v\xE0o\n'); while x ~= fprintf('\nNh\x1EADp gi\xE1 tr\x1ECB \x63\x1EE7\x61 \x62i\x1EBFn kh\x1EDBp:\n'); for i = 1:6 fprintf('Kh\x1EDBp %d:', i); y = input(' ')*pi/180; if isempty(y) th(i) = 0; else th(i) = y; end end fprintf('\nM\x61 tr\x1EADn \x62i\x1EBFn \x111\x1ED5i: \n'); T06 = f(th(1), th(2), th(3), th(4), th(5), th(6)); disp(T06); fprintf('V\x1ECB tr\xED t\x61y m\xE1y: \n'); hand = T06 * [0;0;1;1]; disp(hand); fprintf('\x43\xF3 ti\x1EBFp t\x1EE5\x63 nh\x1EADp kh\xF4ng: '); x = input(''); if isempty(x) x = 0; end end Tính tốn ma trận Jacoby 3.1 Xác định ma trận Tni n Áp dụng công thức Tni = Aj với Aj ma trận chuyển đổi toạ độ phần j =i +1 2.3 3.2 Xác định ma trận H J Vì robot EP4000D có khớp quay nên ma trận H J R 66 có cột có dạng: 11 i n y i p x − i nx i p y i i i i o y p x − ox p y i i i i H X a y px − ax p y H J i +1 = = i i +1 nz i oz i az H J = H J1 3.3 H J H J Xác định ma trận Jacoby Ma trận Jacoby J xác định từ 0 R J = n 0 J theo công thức: H J Rn nx Với Rn = ny nz 3.4 H ox oy oz ax ay az Chương trình Matlab tính tốn ma trận Jacoby % Xác định mà trận Jacoby thông qua ma Tn = cell(6,1); JH = cell(1,1); for i=6:-1:1 if i==6 Tn{i} = A{i}; else Tn{i} = A{i}*Tn{i+1}; end JH{1}(1,i) = Tn{i}(2,1)*Tn{i}(1,4) JH{1}(2,i) = Tn{i}(2,2)*Tn{i}(1,4) JH{1}(3,i) = Tn{i}(2,3)*Tn{i}(1,4) JH{1}(4,i) = Tn{i}(3,1); JH{1}(5,i) = Tn{i}(3,2); JH{1}(6,i) = Tn{i}(3,3); end fprintf('M\x61 tr\x1EADn JH: \n'); disp(JH{1}); trận JH - Tn{i}(1,1)*Tn{i}(2,4); - Tn{i}(1,2)*Tn{i}(2,4); - Tn{i}(1,3)*Tn{i}(2,4); J = [Tn{1}(1:3,1:3),zeros(3);zeros(3),Tn{1}(1:3,1:3)]*JH{1}; fprintf('M\x61 tr\x1EADn Jacoby J: \n'); disp(J); 12 Tính tốn động học đảo vị trí Robot 4.1 Tính ma trận nghịch đảo ma trận vị trí khớp Từ ma trận vị trí khớp xây dựng phần động học thuận, sử dụng lệnh inv matlab ta tìm ma trận nghịch đảo: B1=simplify(inv(A1)) Ta thu kết quả: 𝑐𝑜𝑠 𝑖 𝑠𝑖𝑛 𝑖 −𝑎1 0 −1 𝐵1 = [ ] −𝑠𝑖𝑛 𝑖 𝑐𝑜𝑠 𝑖 0 0 B2=simplify(inv(A2)); 𝑐𝑜𝑠 𝑖 𝑠𝑖𝑛 𝑖 𝐵2 = [ 0 𝑠𝑖𝑛 𝑖 −𝑐𝑜𝑠 𝑖 0 0 −1 0 ] B3=simplify(inv(A3)); 𝑐𝑜𝑠 𝑖 𝐵3 = [ 𝑠𝑖𝑛 𝑖 𝑠𝑖𝑛 𝑖 − 𝑐𝑜𝑠 𝑖 0 0 −𝑎3 ] B4=simplify(inv(A4)); 𝑐𝑜𝑠 𝑖 𝑠𝑖𝑛 𝑖 0 𝐵4 = [ − 𝑠𝑖𝑛 𝑖 𝑐𝑜𝑠 𝑖 0 0 −1 𝑑4 ] 0 B5=simplify(inv(A5)); 𝑐𝑜𝑠 𝑖 𝐵5 = [ 𝑠𝑖𝑛 𝑖 𝑠𝑖𝑛 𝑖 − 𝑐𝑜𝑠 𝑖 0 0 𝑠𝑖𝑛 𝑖 𝑐𝑜𝑠 𝑖 0 0 0 ] B6=simplify(inv(A6)); 𝑐𝑜𝑠 𝑖 − 𝑠𝑖𝑛 𝑖 𝐵6 = [ 0 0 ] −𝑑6 13 Tính tốn động học ngược robot 4.2 Giả sử biết trước ma trận : 𝑛𝑥 𝑛 𝑇60 = 𝐴1 𝐴2 𝐴3 𝐴4 𝐴5 𝐴6 = [ 𝑦 𝑛𝑧 𝑜𝑥 𝑜𝑦 𝑜𝑧 𝑎𝑥 𝑎𝑦 𝑎𝑧 𝑝𝑥 𝑝𝑦 ] 𝑝𝑧 ⇒ 𝐿1 = 𝐴1−1 𝑇60 = 𝐴2 𝐴3 𝐴4 𝐴5 𝐴6 = 𝑅1 (∗) • Tính i1 Từ vế (*) Ta thấy 𝑅1(3,4) 𝑅1(3,3) = d6 = 𝐿1(3,4) 𝐿1(3,3) = 𝑝𝑦.𝑐1 − 𝑝𝑥.𝑠1 𝑎𝑦.𝑐1 − 𝑎𝑥.𝑠1 Ta giải nghiệm i1 : i1 = atan2(py-a6.ay;px-d6.ax) i1 = atan2(py-a6.ay;px-d6.ax) + π Sau tìm q1 ta biết ma trận L1 𝑓𝑥 𝑓 L1 = [ 𝑦 𝑓𝑧 𝑔𝑥 𝑔𝑦 𝑔𝑧 ℎ𝑥 ℎ𝑦 ℎ𝑧 𝑘𝑥 𝑘𝑦 ] 𝑘𝑧 • Tính i2, i3 Sau ta xét tiếp L2 = L1.𝐴6−1 ; R2 =A2.A3.A4.A5 Sử dụng matlab để nhân, ta thấy: L2(1,4)= kx - d6.hx L2(2,4)= ky - d6.hy R2(1,4)= - d4.s23 + a3.c23 + a2.s2 R2(2,4)= d4.c23 + a3.s23 + a2.s2 (d4 - a3).c23 + (a3 + d4).s23= ky - d6.hy – kx + d6.hx 14 A.c23 + B.s23 = C Do : i23 = atan2(B,A) + atan2 (± √𝐴2 + 𝐵2 − 𝐶 ,C) i2 = atan2 (ky – d6.hy + d4.c23- a3.c23 , kx – d6.hx – d4.c23 - a3.s23) i23=i2 - i3 => i3= i2 – i23= atan2 (ky – d6.hy + d4.c23- a3.c23 , kx – d6.hx – d4.c23 a3.s23) - atan2 (ky – d6.hy + d4.c23- a3.c23 , kx – d6.hx – d4.c23 - a3.s23) • Tính i4,i5,i6 + Khi xác định i1,i2,i3 ta hồn tồn xác định T36 𝑓1𝑥 𝑓1 Giả sử T36 = [ 𝑦 𝑓1𝑧 𝑔1𝑥 𝑔1𝑦 𝑔1𝑧 ℎ1𝑥 ℎ1𝑦 ℎ𝑧 𝑘1𝑥 𝑘1𝑦 ] 𝑘1𝑧 + L3 = 𝐴4−1 𝑇36 ; R3 = T46 + L3=R3 *Ta thấy: + L3(2,3)=h1z + R3(2,3)= -c5 c5=-h1z = t Ta có giá trị i5= ATAN2(-t;±√1 − 𝑡 ) i5= π - ATAN2(-t;±√1 − 𝑡 ) + L3(3,3)= h1y.c4 - h1x.s4=0 => i4 = atan2 (h1y , h1x) i4 = (h1y, h1x) + π + L3(3,1)= s6=R3(3,1)= f1y.c4 - f1x.s4 + L3(3,2)=c6=R(3,2)= g1y.c4 -g1x.s4 + i6 = atan2(f1y.c4 – f1x.s4 , g1y.c4 -g1x.s4) Thiết kế quỹ đạo chuyển động cho khớp Robot theo quỹ đạo dạng bậc 15 THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG DẠNG BẬC - Xem xét cho khớp: • Quỹ đạo khớp quay dạng bậc có dạng: (t ) = a0 + a1t + a2t + a3t • Quỹ đạo tốc độ là: (t ) = a1 + 2a2t + 3a3t • Giả sử khớp quay thứ i yêu cầu di chuyển từ góc khớp ban đầu đến f thời gian t f • Hai ràng buộc giá trị góc khớp ban đầu giá trị góc khớp cuối là: (0) = 0 (t f ) = f • Hai ràng buộc tốc độ khớp ban đầu tốc độ góc khướp cuối là: (0) = (t f ) = f • Hệ phương trình ràng buộc là: (0) = a0 = (t f ) = a0 + a1t f + a2t f + a3t f = f (0) = a1 = (t ) = a + 2a t + 3a t = f f f f • Giải hệ phương trình ta thu được: a0 = 0 a1 = a2 = ( f − 0 ) − − f tf tf tf a3 = − ( f − 0 ) + ( f − 0 ) tf tf 16 - Vậy đạo quỹ cho khớp qi là: 3 qi (t ) = i (t ) = 0 + 0t + ( f − 0 ) − − f t + − ( f − ) + ( f + ) t tf t f tf t f t f Mô phỏng động lực học Robot EP4000D a Thiết kế Robot Để mô động lực học robot, chúng em sử dụng thư viện Simscape Multibody Simulink Matlab Thư viện cho phép mô động lực học vật rắn cách tạo nối, khớp nối từ khối hình (có thể tùy chỉnh hàm phân phối trọng lượng) Trong mô phỏng, chúng em tối giản nối, khớp nối robot dạng khối (đảm bảo trục quay, hệ tọa độ giống với thông số thực tế robot; mỗi nối biểu diễn màu) hình sau: Hình 6.1 Hình 3D mô robot 17 Các khối Simulink mô robot mơ tả hình sau: Hình 6.2 Mô robot Simulink Các nối robot có khối lượng, đặt mơ men vào trục quay, biến khớp robot thay đổi theo mô men đặt Kết 18 mô cho thấy biến khớp thay đổi theo mô men đặt vào trục quay theo lý thuyết 19 b Thiết kế điều khiển Trong hình 6.1 mơ robot, chúng em đặt trọng trường hướng ngược trục Z Như vậy, robot có khối lượng (các nối có khối lượng), biến khớp có thêm mô men trọng lực gây Để điều khiển biến khớp điều kiện có trọng trường vậy, chúng em sử dụng điều khiển PD bù trọng trường Bộ điều khiển PD bù trọng trường có vector mơ men điều khiển sau: M = K p * E − K D * Q + G (Q ) Trong ma trận K P , K D ma trận chéo có tất thành phần đường chéo dương, E vector sai lệch, Q vector vận tốc biến khớp, Q (G ) vector thành phần trọng trường hệ phương trình động lực học robot Vì tổng P robot không chứa thành phần Q nên vector tính theo cơng thức sau: Q (G ) T P P P P P P G = , , , , , 1 3 5 6 Khi biết trước trọng tâm khối lượng nối, dễ dàng tính P theo biến khớp Q Với robot EP4000D hình 6.1 (và hình 6.2 - file mơ robot.slx), P tính sau: %Tính P của robot EP4000d cho mô phỏng syms x1 x2 x3 x4 x5 x6; A1=[cos(x1) -sin(x1) 0.55*cos(x1);sin(x1) cos(x1) 0.55*sin(x1);0 -1 0;0 0 1]; A2=[cos(x2) sin(x2) 1.1*cos(x2);sin(x2) -cos(x2) 1.1*sin(x2);0 -1 0;0 0 1]; A3=[cos(x3) sin(x3) 0.25*cos(x3);sin(x3) -cos(x3) 0.25*sin(x3);0 0;0 0 1]; A4=[cos(x4) -sin(x4) 0;sin(x4) cos(x4) 0;0 -1 2.135;0 0 1]; A5=[cos(x5) sin(x5) 0;sin(x5) -cos(x5) 0;0 0;0 0 1]; A6=[cos(x6) -sin(x6) 0;sin(x6) cos(x6) 0;0 0.255;0 0 1]; P1=9.8*(A1*A2*[0.625;0;0;1]*85.5+A1*A2*[0;0;0;1]*20.3575+A1*A2*A3*[0;0;1;1]*153+A1*A2* A3*[0;0;0;1]*45+A1*A2*A3*A4*[0;0.21;0;1]*8.14301+A1*A2*A3*A4*A5*[0;0;0; 1]*6.78584+A1*A2*A3*A4*A5*A6*[0;0;-0.0675;1]*6.48); P=P1(3) 20 Kết thu 𝑃(𝑄), từ tính vector 𝐺(𝑄) theo cơng thức Để đơn giản, ma trận 𝐾𝑃 , 𝐾𝐷 chọn 𝐾𝑃 = 𝐾𝐷 = 5000 ∗ 𝐼 (𝐼 ma trận đơn vị) Bộ điều khiển mô tả Simulink sau: Hình 6.3 Cấu trúc điều khiển PD bù trọng trường Mơ tồn hệ thống gồm robot điều khiển Simulink hình sau: Hình 6.4 Mơ điều khiển robot Kết mô điều khiển biến khớp 𝑄 robot tác động trọng trường cho kết tốt, biến khớp 𝑄 xác lập sau khoảng giây với đồ thị đáp ứng biến khớp 𝜃2 hình sau: 21 Hình 6.5 Kết mơ điều khiển biến khớp 𝜃2 Mô điều khiển robot với 𝑄𝑆𝑃 = (−𝜋/3, 𝜋/3, 𝜋/3, 𝜋/3, 𝜋/4, 𝜋/6)𝑇 cho kết tốt, biến khớp xác lập với đồ thị hình 6.5, kết mơ robot mơ tả hình sau: Hình 6.6 Hình 3D mơ điều khiển robot 22 DANH MỤC TÀI LIỆU THAM KHẢO Robot công nghiệp - GS.TSKH Nguyễn Thiện Phúc Trang web: https://motoman.com/enus/products/robots/industrial/assembly-handling/ep-series/ep4000d Datasheet - https://motoman.com/getmedia/a164a96d-f479-407a-95e22bbf86788edc/163305-1CD.pdf.aspx Instruction Manual - https://motoman.com/getmedia/a65d2767-0d34-4393b35e-54d52837ed0c/EP4000D_EPH4000D-DX100.pdf.aspx 23