Báo cáo kỹ thuật robot robot abb 910 sc

ĐẠI HỌC BÁCH KHOA HÀ NỘI TRƯỜNG ĐIỆN – ĐIỆN TỬ ********************** BÁO CÁO KỸ THUẬT ROBOT (Mã học phần: EE4344) ROBOT ABB 910 SC Giảng viên hướng dẫn: PGS TS Nguyễn Phạm Thục Anh Nhóm sinh viên thực hiện: Nhóm 05 STT Họ tên Hồ Tuấn Anh Nguyễn Đình Quốc Khánh Trần Xuân Linh Trần Văn Lợi Nguyễn Quang Sáng Ma Doãn Viễn MSSV 20191670 20191907 20191926 20191929 20192052 20192171 Hà Nội, tháng 06 năm 2023 NHIỆM VỤ CÁC THÀNH VIÊN Nội dung Chương Tổng quan ROBOT ABB 910SC Chương Động học thuận vị trí xây dựng phần mềm tính tốn cho ROBOT ABB 910SC Chương Động học ngược vị trí xây dựng phần mềm tính toán cho ROBOT ABB 910SC Chương Ma trận Jacobi xây dựng phần mềm tính tốn cho ROBOT ABB 910SC Chương Thiết kế quỹ đạo chuyển động cho khớp robot theo quỹ đạo dạng bậc Chương Thiết kế điều khiển chuyển động cho robot theo thuật tốn PID Chương Xây dựng mơ hình động lực học cho đối tượng Toolbox Simscape/Matlab Họ tên MSSV Hồ Tuấn Anh 20191670 Ma Doãn Viễn 20192171 Trần Xuân Linh 20191926 Nguyễn Đình Quốc Khánh 20191907 Trần Văn Lợi 20191929 Nguyễn Quang Sáng 20192052 Nguyễn Quang Sáng 20192052 MỤC LỤC CHƯƠNG TỔNG QUAN VỀ ROBOT ABB 910SC 1.1 Giới thiệu chung 1.1.1 Tổng quan 1.1.2 Các đặc trưng IRB 910SC 1.2 Các thông số kĩ thuật 1.2.1 Cấu hình 1.2.2 Thông số kỹ thuật CHƯƠNG ĐỘNG HỌC THUẬN VỊ TRÍ VÀ XÂY DỰNG PHẦN MỀM TÍNH TỐN CHO ROBOT ABB 910SC 2.1 Động học thuận vị trí Robot ABB 910SC 2.1.1 Xác định khớp hệ trục tọa độ 2.1.2 Xác định thông số động học Robot 2.1.3 Xác định ma trận biến đổi đồng T 2.2 Xây dựng phần mềm tính toán 2.2.1 Code m – file 2.2.2 Kết tính tốn 10 CHƯƠNG ĐỘNG HỌC NGƯỢC VỊ TRÍ VÀ XÂY DỰNG PHẦN MỀM TÍNH TỐN CHO ROBOT ABB 910SC 11 3.1 Động học ngược vị trí Robot ABB 910SC 11 3.1.1 Động học ngược vị trí 11 3.1.2 Động học ngược vị trí Robot ABB 910SC 12 3.2 Xây dựng phần mềm tính tốn 13 3.2.1 Code m – file 13 3.2.2 Kết tính tốn 14 CHƯƠNG MA TRẬN JACOBI VÀ XÂY DỰNG PHẦN MỀM TÍNH TỐN CHO ROBOT ABB 910SC 15 4.1 Ma trận Jacobi 15 4.1.1 Thuật toán ma trận J 16 4.1.2 Xác định ma trận Jacobi 17 4.2 Xây dựng phần mềm tính tốn 18 CHƯƠNG THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO CÁC KHỚP ROBOT THEO QUỸ ĐẠO DẠNG BẬC 20 5.1 Quỹ đạo dạng đa thức bậc 20 5.1.1 Quỹ đạo dạng đa thức bậc 20 5.1.2 5.2 Quỹ đạo Robot ABB 910SC 20 Xây dựng phần mềm tính tốn vẽ đồ thị 21 CHƯƠNG THIẾT KẾ ĐIỀU KHIỂN CHUYỂN ĐỘNG CHO ROBOT THEO THUẬT TOÁN PID 22 6.1 Tính tốn thiết kế 22 6.1.1 Động nối 22 6.1.2 Hàm Lagrange 24 6.1.3 Mô men tác động lên nối 24 6.1.4 Bộ điều khiển 25 6.2 Mô 25 CHƯƠNG XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG TRÊN TOOLBOX SIMSCAPE/MATLAB 28 DANH MỤC BẢNG BIỂU Bảng 1.1 – Vị trí phân loại khớp Bảng 1.2 – Thông số Bảng 1.3 – Thơng số kích thước Bảng 1.4 – Vùng làm việc Bảng 1.5 – Giải thích ký hiệu Bảng 1.5 – Hiệu suất độ xác Bảng 2.1 – Bảng D - H DANH MỤC HÌNH ẢNH Hình 1.1 – Robot IRB 910SC Hình 1.2 – Các trục thao tác Robot Hình 1.3 – Hình chiếu (từ lên) Hình 1.4 – Hình chiếu đứng Hình 1.5 – Hình chiếu (từ xuống) Hình 1.6 – Vùng làm việc: (a) IRB 910SC-3/0.45; (b) IRB 910SC-3/0.55; (c) IRB 910SC-3/0.65 Hình 2.1 – Các khớp Robot ABB 910SC Hình 2.2 – Xác định hệ trục tọa độ Hình 2.5 – Kết tính tốn với thơng số 10 Hình 3.1 – Phép quay trục (A) & (B) 12 Hình 3.2 - Kết tính tốn với thơng số 14 Hình 4.1 – Mơ tả trình hoạt động Robot 15 Hình 4.2 – Kết tính tốn với thơng số 19 Hình 6.1 - Hệ trục tọa độ tham số khớp nối robot Scara IRB 910SC 22 Hình 6.2 – Mơ hình 25 Hình 6.3 – Đáp ứng vị trí khớp 26 Hình 6.4 - Đáp ứng vị trí khớp 26 Hình 6.5 - Đáp ứng vị trí khớp 27 Hình 6.6 - Đáp ứng vị trí khớp 27 Hình 7.1 – Mơ hình Matlab 28 Hình 7.2 – Mơ hình Robot 28 CHƯƠNG TỔNG QUAN VỀ ROBOT ABB 910SC 1.1 Giới thiệu chung 1.1.1 Tổng quan Hình 1.1 – Robot IRB 910SC IRB 910SC ABB robot SCARA dạng cánh tay, có khả hoạt động vùng với chân đế cố định Robot lý tưởng cho công việc lắp ráp phận nhỏ, xử lý nguyên liệu hay kiểm tra sản phẩm 1.1.2 Các đặc trưng IRB 910SC a) Các biến thể IRB 910SC có biến thể: IRB 910SC –3/0.45, IRB 910SC – 3/0.55 IRB 910SC – 3/0.65 Cấu trúc biến thể giống nhau, khác độ dài nối tầm với 450 mm, 550 mm 650 mm - - - b) Các thị trường hướng đến 3C: + Lắp ráp kiểm tra chip tùy chỉnh, kiểm tra bảng tự động, lắp ráp linh kiện điện tử hàng không vũ trụ + Kiểm tra bảng mạch PC, tự động làm thành phần bảng mạch Đóng gói thực phẩm: + Đóng gói thịt, đóng gói bim bim, cắt dán hộp, đóng gói thành phần nhựa c) Các ứng dụng Lắp ráp thành phần nhỏ: + Lắp vít + Gắn thêm thành phần - - 1.2 + Lắp ráp, tháo Xử lý nguyên liệu: + Lấy đặt sang vị trí khác + Xử lý phần nhỏ + Sắp xếp Kiểm tra + Kiểm tra sản phẩm + Chạy thử + Kiểm sốt chất lượng Các thơng số kĩ thuật 1.2.1 Cấu hình Hình 1.2 – Các trục thao tác Robot Bảng 1.1 – Vị trí phân loại khớp Vị trí A B C D Trục Trục Trục Trục Trục Khớp Quay Quay Quay Tịnh tiến 1.2.2 Thông số kỹ thuật a) Các thơng số định mức IRB 910SC có phiên gắn mặt mặt phẳng Bảng 1.2 – Thông số Loại robot Khả mang tải định mức Khả mang tải tối đa Tầm với Trọng lượng b) IRB 910SC3/0.45 IRB 910SC3/0.55 IRB 910SC3/0.65 Kích thước kg kg 0.45 m 24.5 kg kg kg 0.55 m 25 kg kg kg 0.65 m 25.5 kg Hình 1.3 – Hình chiếu (từ lên) Hình 1.4 – Hình chiếu đứng %% Xác định d3 d3 = -pz; %% Xác định Theta4 Theta4_1 = Theta1_1 + Theta2_1 - atan2d(ny, nx); Theta4_2 = Theta1_2 + Theta2_2 - atan2d(ny, nx); %% Đưa kết fprintf('\n'); disp('Có nghiệm thỏa mãn:'); fprintf('\n'); disp('Nghiệm thứ nhất: '); nghiem1 = [Theta1_1 Theta2_1 d3 Theta4_1] disp('Nghiệm thứ hai: '); nghiem2 = [Theta1_2 Theta2_2 d3 Theta4_2] 3.2.2 Kết tính tốn Hình 3.2 - Kết tính tốn với thông số 14 CHƯƠNG MA TRẬN JACOBI VÀ XÂY DỰNG PHẦN MỀM TÍNH TỐN CHO ROBOT ABB 910SC 4.1 Ma trận Jacobi yn  qn z0  q2 zn xn y0  q1 x0 Hình 4.1 – Mơ tả q trình hoạt động Robot 𝛿𝑄 = [𝛿𝑞 𝛿𝑞 𝛿𝑞 … 𝛿𝑞 ] - Ta có: - Ma trận Jacobi định nghĩa: 𝛿𝑋 = 𝛿𝑝 𝛿𝑝 𝛿𝑝 𝛿𝜙 𝛿𝜙 𝛿𝜙 ⇒ 𝑋̇ = 𝐽 𝑄̇ ⇒ 𝐽 = 𝐽 - → = 𝐽 lim → 𝑋= 𝑝 𝑝 𝑝 𝜙 𝜙 𝜙 𝑄 = [𝑞 𝑞 𝑞 … 𝑞 ] với 𝜕𝑝 ⎡ ⎢ 𝜕𝑝 ⎢ 𝜕𝑝 =⎢ 𝜕𝜙 ⎢ ⎢𝜕𝜙 ⎣ 𝜕𝜙 lim /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 𝜕𝑝 𝜕𝑝 𝜕𝑝 𝜕𝜙 𝜕𝜙 𝜕𝜙 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 … … … … … … 𝜕𝑝 𝜕𝑝 𝜕𝑝 𝜕𝜙 𝜕𝜙 𝜕𝜙 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 /𝜕𝑞 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (4.1) Việc xác định ma trận Jacobi theo công thức (4.1) phức tạp robot có số bậc tự lớn (nDOF > 6) Mặt khác với số bậc tự lớn, ta có 𝑋 = 𝛿𝑝 𝛿𝑝 𝛿𝑝 𝛿𝜙 𝛿𝜙 𝛿𝜙 ô Do vậy, ta sử dụng phương ườ pháp xác định ma trận Jacobi thơng qua sử dụng thuật tốn ma trận 𝐽 15 4.1.1 Thuật toán ma trận 𝐉𝐇 - - a) Xác định ma trận 𝑇 Ma trận 𝑇 xác định: 𝑛 𝑜 𝑎 𝑝 ⎡ ⎤ 𝑛 𝑜 𝑎 𝑝 ⎥ 𝑇 =⎢ (4.2) ⎢𝑛 𝑜 𝑎 𝑝 ⎥ ⎣0 0 1⎦ = 𝐴 (𝜃 ) … 𝐴 (𝜃 ) Đối với Robot ABB 910SC có bậc tự  cần xác định ma trận 𝑇 (𝑖 = → 3): Từ ma trận 𝐴 xác định phương trình (2.8) đến (2.11), ta xác định: 𝑇 =𝐴 𝐴 𝐴 𝐴 𝑐(𝜃 + 𝜃 − 𝜃 ) 𝑠(𝜃 + 𝜃 − 𝜃 ) ) = 𝑠(𝜃 + 𝜃 − 𝜃 ) −𝑐(𝜃 + 𝜃 − 𝜃 0 0 𝑎 𝑐𝜃 + 𝑎 𝑐(𝜃 + 𝜃 ) 𝑎 𝑠𝜃 + 𝑎 𝑠(𝜃 + 𝜃 ) −1 −𝑑 - 𝑐(𝜃 − 𝜃 ) 𝑠(𝜃 − 𝜃 ) 𝑎 𝑐𝜃 ( ) 𝑎 𝑠𝜃 𝑇 = 𝑠(𝜃 − 𝜃 ) −𝑐 𝜃 − 𝜃 0 −1 −𝑑3 0 𝑐𝜃 −𝑠𝜃 0 𝑠𝜃 𝑐𝜃 0 𝑇 = 0 𝑑 0 𝑐𝜃 −𝑠𝜃 0 𝑠𝜃 𝑐𝜃 0 𝑇 =𝐴 = 0 0 0 b) Xác định ma trận 𝐽 Nếu khớp thứ (i+1) khớp trượt  Biến khớp 𝑑 - 𝜕𝑝 𝜕𝑝 𝜕𝑝 =𝑛 ; =𝑜 ; =𝑎 𝜕𝑑 𝜕𝑑 𝜕𝑑 𝜕𝜙 𝜕𝜙 𝜕𝜙 = = =0 𝜕𝑑 𝜕𝑑 𝜕𝑑 𝜕𝑋 𝐽 = 𝜕𝑄 Nếu khớp thứ (i+1) khớp quay  Biến khớp 𝜃 𝜕𝑝 =𝑛 𝑝 −𝑛 𝑝 ; 𝜕𝜃 𝜕𝑝 =𝑜 𝑝 −𝑜 𝑝 ; 𝜕𝜃 (4.3) (4.4) (4.5) (4.6) (4.7) (4.8) (4.9) (4.10) 16 - 𝜕𝑝 =𝑎 𝑝 −𝑎 𝑝 𝜕𝜃 𝜕𝜙 𝜕𝜙 𝜕𝜙 =𝑛 ; =𝑜 ; 𝜕𝜃 𝜕𝜃 𝜕𝜃 Đối với Robot ABB 910SC có bậc tự do: + Khớp khớp tịnh tiến: 𝜕𝑝 𝜕𝑝 =𝑛 =0; = 𝑜 = 0; 𝜕𝑑 𝜕𝑑 𝜕𝜙 𝜕𝜙 𝜕𝜙 = = 𝜕𝑑 𝜕𝑑 𝜕𝑑 + Khớp 1, 2, khớp quay: 𝜕𝑝 𝜕𝜃 𝑖 𝜕𝑝 𝜕𝜃 𝑛 𝑝 −𝑛 𝑝 = 𝑎 𝑠(𝜃 − 𝜃 ) − 𝑎 𝑠𝜃 𝑛 𝑝 −𝑛 𝑝 = −𝑎 𝑠𝜃 𝑛 𝑝 −𝑛 𝑝 =0 - 𝑜 = −𝑎 𝑐𝜃 𝑜 = −𝑎 𝑐𝜃 𝑜 =0 𝑝 −𝑜 𝑝 − 𝑎 𝑐(𝜃 − 𝜃 ) 𝑝 −𝑜 𝑝 𝑝 −𝑜 𝑝 (4.11) =𝑎 𝜕𝑝 =𝑎 =1 𝜕𝑑 =0 𝜕𝑝 𝜕𝜃 𝜕𝜙 𝜕𝜃 𝜕𝜙 𝜕𝜃 𝑎 𝑝 −𝑎 𝑝 =0 𝑎 𝑝 −𝑎 𝑝 =0 𝑎 𝑝 −𝑎 𝑝 =0 𝑛 =0 𝑛 =0 𝑛 =0 𝜕𝜙 𝜕𝜃 𝑎 = −1 𝑎 = −1 𝑎 =1 𝑜 =0 𝑜 =0 𝑜 =0 Ma trận 𝐽 : 𝐽 × 𝑎 𝑠(𝜃 − 𝜃 ) − 𝑎 𝑠𝜃 ⎡ −𝑎 𝑐𝜃 − 𝑎 𝑐(𝜃 − 𝜃 ) 𝜕𝑋 ⎢ = =⎢ 𝜕𝑄 ⎢ ⎢ ⎣ −1 −𝑎 𝑠𝜃 −𝑎 𝑐𝜃 0 −1 0 0 0 ⎤ ⎥ 0⎥ 0⎥ 0⎥ 1⎦ 4.1.2 Xác định ma trận Jacobi - Ma trận Jacobi xác định theo công thức: - 𝑅 × ×𝐽 × 𝑅 Với Robot ABB 910SC, ma trận Jacobi tính: 𝐽= 𝑅 × 𝑠(𝜃 +𝜃 − 𝜃 ) −𝑐(𝜃 +𝜃 − 𝜃 ) 0 0 𝐽= 𝑐(𝜃 +𝜃 − 𝜃 ) ⎡ 𝑠(𝜃 +𝜃 − 𝜃 ) ⎢ =⎢ ⎢ ⎢ ⎣ 0 × ×𝐽 𝑅 0 0 −1 0 𝑐(𝜃 +𝜃 − 𝜃 ) 𝑠(𝜃 +𝜃 − 𝜃 ) 0 (4.12) (4.13) 0 𝑠(𝜃 +𝜃 − 𝜃 ) −𝑐(𝜃 +𝜃 − 𝜃 ) 17 ⎤ ⎥ 0⎥ 𝐽 0⎥ 0⎥ −1⎦ −𝑎 𝑠(𝜃 + 𝜃 ) − 𝑎 𝑠𝜃 ⎡ ( ) ⎢ 𝑎 𝑐 𝜃 + 𝜃 + 𝑎 𝑐𝜃 =⎢ ⎢ ⎢ ⎣ 4.2 −𝑎 𝑠(𝜃 + 𝜃 ) 𝑎 𝑐(𝜃 + 𝜃 ) 0 0 −1 0 0 ⎤ 0⎥ 0⎥ 0⎥ 0⎥ −1⎦ Xây dựng phần mềm tính tốn a) Code m – file clear all fprintf('\n'); disp('Nhập vào ma trận thông số bảng D - H'); theta = input('Nhập "ma trận" giá trị theta_i (deg): '); alpha = input('Nhập "ma trận" giá trị alpha_i (deg): '); a = input('Nhập "ma trận" giá trị a_i (mm): '); d = input('Nhập "ma trận" giá trị d_i (mm): '); theta1 = theta(1); theta2 = theta(2); alpha1 = alpha(1); alpha2 = alpha(2); d1 = d(1); d2 = d(2); d3 = d(3); d4 = a1 = a(1); a2 = a(2); a3 = a(3); a4 = theta3 = theta(3); theta4 = theta(4); alpha3 = alpha(3); alpha4 = alpha(4); d(4); a(4); DH = [theta' alpha' a' d'] A1 = [cosd(theta1) a1*sind(theta1); A2 = [cosd(theta2) a2*sind(theta2); A3 = [1 0 0; A4 = [cosd(theta4) 0 1]; T04 T14 T24 T34 = = = = -sind(theta1) a1*cosd(theta1); sind(theta1) cosd(theta1) 0 0; 0 1]; sind(theta2) a2*cosd(theta2); sind(theta2) -cosd(theta2) 0 -1 0; 0 1]; 0; 0 d3; 0 1]; -sind(theta4) 0; sind(theta4) cosd(theta4) 0; 0 0; (A1*A2*A3*A4); A2*A3*A4; A3*A4; A4; R04 = T04(1:3, 1:3); JH1 = [T04(2,1)*T04(1,4) - T04(1,1)*T04(2,4); T04(2,2)*T04(1,4) T04(1,2)*T04(2,4); T04(2,3)*T04(1,4) - T04(1,3)*T04(1,4); 0; 0; JH2 = [T14(2,1)*T14(1,4) - T14(1,1)*T14(2,4); T14(2,2)*T14(1,4) T14(1,2)*T14(2,4); T14(2,3)*T14(1,4) - T14(1,3)*T14(1,4); 0; 0; JH3 = [0; 0; 1; 0; 0; 0]; JH4 = [T34(2,1)*T34(1,4) - T34(1,1)*T34(2,4); T34(2,2)*T34(1,4) T34(1,2)*T34(2,4); T34(2,3)*T34(1,4) - T34(1,3)*T34(1,4); 0; 0; -1]; -1]; 1]; JH = [JH1 JH2 JH3 JH4] J = [R04 zeros(3,3); zeros(3,3) R04]*JH 18 b) Kết tính tốn Hình 4.2 – Kết tính tốn với thông số 19 CHƯƠNG THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO CÁC KHỚP ROBOT THEO QUỸ ĐẠO DẠNG BẬC 5.1 Quỹ đạo dạng đa thức bậc 5.1.1 Quỹ đạo dạng đa thức bậc Đúng tên gọi, quỹ đạo khớp robot thiết kế dạng đa thức bậc sau: - Quỹ đạo khớp: Quỹ đạo tốc độ: 𝜃 (𝑡 ) = 𝑎 + 𝑎 𝑡 + 𝑎 𝑡 + 𝑎 𝑡 𝜃̇ (𝑡 ) = 𝑎 + 2𝑎 𝑡 + 3𝑎 𝑡 Giả sử khớp thứ (i) yêu cầu di chuyển từ góc khớp bắt đầu 𝜃 đến góc khớp 𝜃 khoảng thời gian 𝑡 Vậy ta có điều kiện đầu, cuối sau: 𝜃 𝑡 𝜃 (0 ) = 𝜃 & 𝜃̇ (0) = 𝜃̇ 𝜃̇ 𝑡 =𝜃 = 𝜃̇ Các hệ số quỹ đạo xác định sau: ⎧ ⎧ 𝑎 = 𝜃 − 𝜃 − 𝜃̇ − 𝜃̇ ⎪𝑎 = 𝜃 ⎪ 𝑡 𝑡 𝑡 ; ⎨𝑎 = 𝜃̇ ⎨𝑎 = − 𝜃 − 𝜃 + 𝜃 ̇ − 𝜃̇ ⎪ ⎪ 𝑡 𝑡 ⎩ ⎩ 5.1.2 Quỹ đạo Robot ABB 910SC Giả sử thời gian cánh tay robot từ điểm đầu A đến điểm cuối B thời gian 𝑡 (s) vận tốc đầu vận tốc cuối 0, ta xác định quỹ đạo hệ số quỹ đạo sau: - - Dạng quỹ đạo: + Quỹ đạo khớp: 𝜃 (𝑡 ) = 𝑎 + 𝑎 𝑡 + 𝑎 𝑡 + 𝑎 𝑡 + Quỹ đạo tốc độ: 𝜃̇ (𝑡 ) = 𝑎 + 2𝑎 𝑡 + 3𝑎 𝑡 Các hệ số xác định: +𝑎 =𝜃 +𝑎 =0 +𝑎 = 𝜃 −𝜃 +𝑎 =− 𝜃 −𝜃 20 5.2 Xây dựng phần mềm tính tốn vẽ đồ thị a) Code m – file b) Đồ thị quỹ đạo 21 CHƯƠNG THIẾT KẾ ĐIỀU KHIỂN CHUYỂN ĐỘNG CHO ROBOT THEO THUẬT TỐN PID 6.1 Tính tốn thiết kế Trước tiên, để thiết kế điều khiển Robot ta cần phải tính tốn đầy đủ thơng số đối tượng lực, momen với vị trí, vận tốc , gia tốc , từ xây dựng phương trình động học.Và phương pháp sử dụng để nghiên cứu động học robot Lagrange Để thuận tiện tính tốn, ta có cách chọn hệ trục tọa độ sau: Hình 6.1 - Hệ trục tọa độ tham số khớp nối robot Scara IRB 910SC 6.1.1 Động nối - Thanh nối 1: 𝑃 = 𝑐𝜃 𝑃̇ = − 𝑠𝜃 𝜃̇ 𝑠𝜃 𝑐𝜃 𝜃̇ 𝜔 = 0 𝜃̇ 𝑃̇ ⋅ 𝑚 ⋅ 𝑃̇ = 𝑚 𝑙 𝜃̇ ⇒ 𝜔 𝐼 𝜔 = 𝐼 𝜃̇ + Động năng: 1 𝐾 = 𝑃̇ 𝑚 𝑃̇ + 𝜔 𝐼 𝜔 = 𝑚 𝑙 +𝐼 𝜃̇ 2 𝐺 = [0 − ] 𝑔 + Thế năng: 𝑙 ⎡ 𝑐𝜃 ⎤ ⎢2 ⎥ 𝑃 = −𝑚 𝐺 𝑃 = −𝑚 [0 − 𝑔] ⎢𝑙 ⎥=0 𝑠𝜃 ⎢2 ⎥ ⎣ ⎦ 22 - Thanh nối 2: 𝑃 = 𝑙 𝑐𝜃 + 𝑐𝜃 𝑃̇ = −𝑙 𝑠𝜃 𝜃̇ − 𝑙 𝑠𝜃 + 𝑠𝜃 𝜃̇ 𝑠𝜃 𝑙 𝑐𝜃 𝜃̇ + 𝑐𝜃 𝜃̇ 𝜔 = 𝑂 𝑂 𝜃̇ ⇒ 𝑙 𝜃̇ + 𝑃̇ ⋅ 𝑚 ⋅ 𝑃̇ = 𝑚 𝜔 𝐼 𝜃̇ 𝜔 = 𝐼 + 𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 𝜃̇ + Động năng: 𝐾 = [𝑃̇ 𝑚 𝑃̇ + 𝜔 𝐼 𝜔 ] 𝑙 = 𝑚 𝑙 𝜃̇ + 𝜃̇ + Thế năng: 𝑃 = −𝑚 𝐺 𝑃 = −𝑚 [0 - Thanh nối 3: 𝑃 = 𝑙 𝑐𝜃 + 𝑙 𝑐𝜃 𝜃̇ ⎤ ⎥ ⎥=0 ⎥ ⎦ +𝑙 −𝑑 𝑙 𝑐𝜃 𝜃̇ + 𝑙 𝑐𝜃 𝜃̇ − 𝑑̇ + 2𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 + 𝑑̇ 𝜔 = 𝐼 𝜃̇ 𝑃̇ ⋅ 𝑚 ⋅ 𝑃̇ = 𝑚 𝑙 𝜃̇ + 𝑙 𝜃̇ 𝜔 𝐼 + Động năng: 1 𝐾 = 𝑃̇ 𝑚 𝑃̇ + 𝜔 𝐼 𝜔 2 = 𝑚 𝑙 𝜃̇ + 𝑙 𝜃̇ + Thế năng: + 2𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 + 𝑑̇ 𝑃 = −𝑚 𝐺 𝑃 = −𝑚 [0 - 𝐼 + 𝑙 ⎡𝑙 𝑐𝜃 + 𝑐𝜃 ⎢ ] −𝑔 ⎢ 𝑙 𝑙 𝑠𝜃 + 𝑠𝜃 ⎢ ⎣ 𝑙 𝑠𝜃 + 𝑙 𝑠𝜃 𝑃̇ = −𝑙 𝑠𝜃 𝜃̇ − 𝑙 𝑠𝜃 𝜃̇ 𝜔 = 𝑂 𝑂 𝜃̇ ⇒ + 𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 Thanh nối 4: 𝑃 = 𝑙 𝑐𝜃 + 𝑙 𝑐𝜃 + 𝐼 𝜃̇ 𝑙 𝑐𝜃 + 𝑙 𝑐𝜃 𝑙 𝑠𝜃 + 𝑙 𝑠𝜃 − 𝑔] 𝑙 +𝑙 −𝑑 𝑙 = 𝑚 𝑔( + 𝑙 − 𝑑 ) 𝑙 𝑠𝜃 + 𝑙 𝑠𝜃 𝑃̇ = −𝑙 𝑠𝜃 𝜃̇ − 𝑙 𝑠𝜃 𝜃̇ 𝜔 = 𝑂 𝑂 𝜃̇ − 𝜃̇ −𝑑 𝑙 𝑐𝜃 𝜃̇ + 𝑙 𝑐𝜃 𝜃̇ − 𝑑̇ 23 ⇒ 𝑃̇ ⋅ 𝑚 ⋅ 𝑃̇ = 𝑚 𝑙 𝜃̇ + 𝑙 𝜃̇ 𝜔 𝐼 𝜔 = 𝐼 + 2𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 + 𝑑̇ (𝜃̇ − 𝜃̇ ) + Động năng: 𝐾 = + Thế năng: 1 𝑃̇ 𝑚 𝑃̇ + 𝜔 𝐼 𝜔 2 = 𝑚 𝑙 𝜃̇ + 𝑙 𝜃̇ + 𝐼 (𝜃̇ − 𝜃̇ ) 𝑃 = −𝑚 𝐺 𝑃 = −𝑚 [0 + 2𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 + 𝑑̇ 𝑙 𝑐𝜃 + 𝑙 𝑐𝜃 𝑙 𝑠𝜃 + 𝑙 𝑆𝜃 − 𝑔] 𝑙 −𝑑 𝑙 = 𝑚 𝑔( − 𝑑 ) 6.1.2 Hàm Lagrange 𝐿 =∑ 𝐾 −∑ 1 = 𝑚 𝑙 +𝐼 + + + 𝑃 𝑚 𝜃̇ + 𝑚 𝜃̇ + 𝑚 (𝜃̇ − 𝜃̇ ) 𝜃̇ + 𝐼 𝐼 𝐼 𝑙 𝜃̇ + 𝑙 𝜃̇ + 𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 𝑙 𝜃̇ + 𝑙 𝜃̇ + 2𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 + 𝑑̇ 𝑙 𝜃̇ + 𝑙 𝜃̇ + 2𝑙 𝑙 𝜃̇ 𝜃̇ 𝑐𝜃 + 𝑑̇ – 0– 0– 𝑚 𝑔 𝑙 − 𝑚 𝑔( − 𝑑 ) 𝑙 +𝑙 −𝑑 6.1.3 Mô men tác động lên nối - Thanh nối 1: 𝜏 = = [( − ̇ + 𝑚 + 𝑚 ) 𝑙 +(𝑚 + 2𝑚 + 2𝑚 ) 𝑙 𝑙 𝑐𝜃 +𝐼 + 𝐼 + 𝐼 + 𝐼 ]𝜃 ̈ +[( + 𝑚 + 𝑚 ) 𝑙 + ( + 𝑚 + 𝑚 ) 𝑙 𝑙 𝑐𝜃 +𝐼 + 𝐼 + 𝐼 ]𝜃 ̈ - 𝐼 𝜃 ̈ - (𝑚 + 2𝑚 + 2𝑚 ) 𝑙 𝑙 𝑠𝜃 𝜃̇ 𝜃̇ - ( + 𝑚 + 𝑚 ) 𝑙 𝑙 𝑠𝜃 𝜃̇ - Thanh nối 2: 𝜏 = =[( +[( - + 𝑚 +𝑚 +𝑚 )𝑙 +( ̇ − + 𝑚 + 𝑚 ) 𝑙 +( + 𝑚 + 𝑚 ) 𝑙 +𝐼 𝑚 ) 𝑙 𝑙 𝑠𝜃 𝜃̇ Thanh nối 3: + 𝑚 + 𝑚 ) 𝑙 𝑙 𝑐𝜃 +𝐼 + 𝐼 + 𝐼 ]𝜃 ̈ + 𝐼 + 𝐼 ]𝜃 ̈ - 𝐼 𝜃 ̈ + ( + 𝑚 + 24 𝐹 = - − ̇ = (𝑚 + 𝑚 )( 𝑑 ̈ − 𝑔) Thanh nối 4: 𝜏 = ̇ − = 𝐼 ( 𝜃 ̈ −𝜃̈ −𝜃 ̈ ) 6.1.4 Bộ điều khiển Coi động lí tưởng Bộ điều khiển phát tín hiệu điều khiển để tạo moment lên khớp quay lực lên khớp tịnh tiến để khớp quay đến vị trí điểm đặt mong muốn Bộ điều khiển PI tạo khối System Control Với đầu vào giá trị đặt mong muốn giá trị khớp Đầu moment lực tác dụng lên khớp - Bộ điều khiển vị trí sử dụng PID: R = 𝐾 (𝑄 − 𝑄) + 𝐾 ∫(𝑄 − 𝑄) dt Với tham số : 𝐾 =1, 𝐾 = 10 - Vị trí ban đầu khớp giả thiết : 𝑄 =[𝜃 𝜃 𝜃 𝜃 ] = [ 0 0] - Vị trí biến khớp mong muốn đạt : 𝑄 =[𝜃 𝜃 𝜃 𝜃 ] = [60 45 15 30] 6.2 Mơ Hình 6.2 – Mơ hình 25 Hình 6.3 – Đáp ứng vị trí khớp Hình 6.4 - Đáp ứng vị trí khớp 26 Hình 6.5 - Đáp ứng vị trí khớp Hình 6.6 - Đáp ứng vị trí khớp 27 CHƯƠNG XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG TRÊN TOOLBOX SIMSCAPE/MATLAB Hình 7.1 – Mơ hình Matlab Derobot,derobot2: Khối đế robot Link 1: Khớp quay thứ Link 2: Khớp quay thứ Link 3: Khớp tịnh tiến thứ Link4: Khớp quay thứ Hình 7.2 – Mơ hình Robot 28