Tính toán thiết kế hệ thống điều khiển cho mô hình robot viết chữ

Hướng dẫn tính toán và thiết kế hệ thống điều khiển cho mô hình robot viết chữ. Tính toán ma trận quay. Tính toán quỹ đạo chuyển động. Tính toán tĩnh học. tính toán phương trình vi phân chuyển động. Tính toán bộ điều khiển. Tính toán kiểm nghiệm bền cho các khâu.

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ -*** - BÀI TẬP LỚN MÔN HỌC TÍNH TỐN THIẾT KẾ ROBOT Đề tài: TÍNH TỐN THIẾT KẾ ROBOT ỨNG DỤNG VIẾT CHỮ GVHD : PGS TS Phan Bùi Khơi Nhóm : 14 Sinh viên thực : Phạm Tiến Dũng Chử Quang Anh Nguyễn Hoàng Anh Mã lớp : 115841 Hà Nội, 2020 Tính tốn, thiết kế robot ứng dụng viết chữ Lời mở đầu Hiện khoa học kỹ thuật phát triển nhanh, mang lại lợi ích cho người tất lĩnh vực sống Để nâng cao đời sống nhân dân hòa nhập với phát triển chung giới, Đảng nhà nước ta đề mục tiêu đưa đất nước lên thành nước cơng nghiệp hóa, đại hóa Để thực mục tiêu ngành cần quan tâm phát triển ngành khí nói chung điện tử nói riêng đóng vai trị quan trọng việc sản xuất thiết bị công cụ (máy móc, robot ) ngành kinh tế Muốn thực việc phát triển ngành khí cần đẩy mạnh đào tạo đội ngũ cán kĩ thuật có trình độ chuyên môn đáp ứng yêu cầu công nghệ tiên tiến, cơng nghệ tự động hóa theo dây chuyền sản xuất Đóng góp vào phát triển nhanh chóng khoa học cơng nghiệp, tự động hóa đóng vai trị vơ quan trọng Vì cơng nghệ tự động hóa đầu tư phát triển mạnh mẽ Trong cơng nghiệp nói chung, việc máy móc tự động dần thay người trở thành xu tất yếu Nhằm tạo hệ thống điều khiển cho robot công nghiệp phục vụ công việc nghiên cứu đưa vào thực tiễn giúp nâng cao suất lao động hay giảm tải cơng việc nhỏ Nội dung đề tài nhóm em thiết kế mơ hình robot ứng dụng viết chữ Trong thực đề tài không tránh khỏi sai sót, mong nhận đóng góp Thầy bạn để nhóm em sửa chữa khắc phục Cuối nhóm sinh viên chúng em xin trân thành cảm ơn bảo tận tình Thầy - PGS TS Phan Bùi Khôi giúp nhóm em học nhiều điều giúp em hồn thành tốt đề tài mơn học Chúng em xin trân trọng cảm ơn! Nhóm sinh viên thực NHĨM 14 NHĨM 14 Tính tốn, thiết kế robot ứng dụng viết chữ BẢNG PHÂN CÔNG ĐÁNH GIÁ CƠNG VIỆC STT Họ tên Nguyễn Hồng Anh MSSV 20160133 Nhiệm vụ • • • • • • Chử Quang Anh 20160038 • • • • • Phạm Tiến Dũng 20160701 • • • • • Nhóm trưởng Tổng hợp file Word thành viên nhóm Đưa kết luận lựa chọn cấu trúc Thiết kế 3D mơ hình robot Thực thiết kế hệ thống điều khiển Thiết lập bảng GUI mô động học Đánh giá /10 Thực phân tích lựa chọn cấu trúc Thiết kế 3D mơ hình robot Tính chọn hệ dẫn động động khâu Chọn hộp giảm tốc sơ Kiểm nghiệm ứng suất khớp ./10 Thực phân tích lựa chọn cấu trúc Thiết kế quỹ đạo chuyển động Thiết kế quỹ đạo chuyển động chữ M Phân tích trạng thái tĩnh Tính tốn động lực học ./10 NHĨM 14 Tính tốn, thiết kế robot ứng dụng viết chữ MỤC LỤC Phân tích lựa chọn cấu trúc 1.1 Phân tích mục đích ứng dụng robot 1.2 Phân tích yêu cầu kỹ thuật thao tác a) Đối tượng thao tác, dạng thao tác b) Phân tích yêu cầu vị trí c) Yêu cầu hướng khâu thao tác d) Yêu cầu vận tốc gia tốc thao tác e) Yêu cầu không gian thao tác 1.3 Xác định đặc trưng kỹ thuật a) Số bậc tự cần thiết b) Vùng làm việc robot c) Yêu cầu tải trọng 1.4 Các phương án thiết kế cấu trúc robot a) Các phương án thiết kế: 1.5 Thông số kỹ thuật 11 Thiết kế 3D mơ hình robot 13 2.1 Thiết kế 3D 13 2.2 Bản vẽ 2D 13 2.3 Các thơng số đặc trưng hình học, khối lượng 14 Thiết kế quỹ đạo chuyển động 15 3.1 Khảo sát động học 15 a) Tính tốn động học thuận robot 15 b) Tính tốn động học ngược robot 19 3.2 Thiết kế quỹ đạo chuyển động robot 19 a) Dạng quỹ đạo đường thẳng: 20 b) Dạng quỹ đạo cung tròn 23 Phân tích trạng thái tĩnh 26 Tính tốn động lực học 32 5.1 Thiết lập phương trình vi phân chuyển động robot 32 5.2 Tính tốn động lực học ngược 34 Thiết kế hệ thống dẫn động 36 6.1 Thiết kế hệ thống dẫn động 36 a) Tính tốn lực dọc trục 37 NHĨM 14 Tính tốn, thiết kế robot ứng dụng viết chữ b) Tính tốn tải trọng 37 c) Chiều dài trục vít-me 37 d) Chọn đường kính trục vít 37 e) Chọn series 38 f) Tính chọn ổ đỡ 38 6.2 Kiểm bền hệ dẫn động 39 a) Kiểm nghiệm trục vít 39 b) Kiểm nghiệm ổ bi đỡ 40 6.3 Chọn động phù hợp 41 a) Chọn động 41 6.4 Tính chọn hộp giảm tốc 42 6.5 Thiết kế 3D kiểm nghiệm bền khâu Robot 42 Thiết kế hệ thống điều khiển 45 7.1 Thiết kế mơ hình điều khiển 45 a) Sơ đồ xây dựng hệ thống điều khiển cho Robot viết chữ 46 b) Kết mơ hình điều khiển 49 7.2 Mô động học robot GUI 51 a) Liên kết mô hình 3D từ SolidWorks vào Matlab 51 b) Thiết kế giao diện đồ họa điều khiển GUI 51 c) Kết mô động học Robot 53 NHÓM 14 Tính tốn, thiết kế robot ứng dụng viết chữ DANH MỤC HÌNH ẢNH Hình 1-1 Robot ứng dụng viết chữ Hình 1-2 Khơng gian làm việc robot Hình 1-3 Robot với cấu trúc RRT Hình 1-4 Robot với cấu trúc TRR 10 Hình 1-5 Robot với cấu trúc TTT 11 Hình 3-1 Các thông số động học khâu 15 Hình 3-2 Mơ hình robot gắn hệ trục tọa độ theo quy tắc D-H 16 Hình 3-3 Mơ khơng gian làm việc Matlab 19 Hình 3-4 Tọa độ điểm cần thiết chữ “ROBOT” 20 Hình 3-5 Tọa độ điểm cơng tác theo trục 21 Hình 3-6 Quỹ đạo chuyển động điểm công tác không gian 22 Hình 3-7 Vận tốc điểm công tác theo x, y, z 22 Hình 3-8 Gia tốc điểm công tác theo x, y, z 22 Hình 3-9 Vị trí biến khớp theo thời gian 23 Hình 3-10 Tọa độ điểm công tác theo x, y, z 24 Hình 3-11 Quỹ đạo chuyển động điểm công tác khơng gian 24 Hình 3-12 Vị trí biến khớp theo thời gian 24 Hình 6-1 Kết cấu ổ đỡ 36 Hình 6-2 Cataloge Vít-me bi hãng TBI 38 Hình 6-3 Cataloge ổ bi 39 Hình 6-4 Sơ đồ lực dọc trục 40 Hình 7-1 Sơ đồ hệ thống điều khiển 46 Hình 7-2 Mơ hình hệ thống điều khiển Simulink 46 Hình 7-3 Khối QUY DAO DAT 47 Hình 7-4 Khối KHOI PD 47 Hình 7-5 Khối KHOI PHAN TU PHI TUYEN 47 Hình 7-6 Khối KHOI ROBOT 48 Hình 7-7 Khối KET QUA 48 Hình 7-8 Đáp ứng vị trí khớp theo thời gian 49 Hình 7-9 Sai lệch vị trí khớp theo thời gian 49 Hình 7-10 Đáp ứng vị trí khớp theo thời gian 50 Hình 7-11 Sai lệch vị trí khớp theo thời gian 50 Hình 7-12 Đáp ứng vị trí khớp theo thời gian 50 Hình 7-13 Sai lệch vị trí khớp theo thời gian 51 Hình 7-14 Sơ đồ khối robot Simulink 51 Hình 7-15 Mơ hình robot Simulink 51 Hình 7-16 Sơ đồ khối Robot thực mô 52 Hình 7-17 Giao diện đồ họa GUI 52 Hình 7-18 Robot viết chữ “ROBOT” theo quỹ đạo đường thẳng cung tròn kết hợp 53 NHÓM 14 Tính tốn, thiết kế robot ứng dụng viết chữ NỘI DUNG TÍNH TỐN THIẾT KẾ Phân tích lựa chọn cấu trúc 1.1 Phân tích mục đích ứng dụng robot Việc viết chữ, kí tên, trang trí bề mặt chi tiết, bề mặt dụng cụ, vẽ vẽ kĩ thuật đòi hỏi chuẩn mực, độ xác cao đồng hàng loạt chi tiết, mà người khó làm việc Vì với phát triển cơng nghiệp hóa tự động hóa, việc nghiên cứu chế tạo robot viết chữ cần thiết, có ứng dụng thực tiễn lớn ngành công nghiệp Ngồi robot viết chữ cịn ứng dụng để viết thiệp, kí tên, kí văn chứng Từ nhu cầu thực tế đó, nhóm thiết kế robot với mục đích thay việc viết chữ truyền thống người cơng nhân Robot sử dụng khâu trình tự động hóa sản suất sản phẩm, giúp tiến trình sản suất diễn liên tục, tiết kiệm thời gian, tăng suất cho nhà máy đảm bảo chất lượng sản phẩm Hình 1-1 Robot ứng dụng viết chữ 1.2 Phân tích yêu cầu kỹ thuật thao tác a) Đối tượng thao tác, dạng thao tác Đối tượng thao tác robot mặt phẳng có khả viết chữ, vẽ hình lên trên, ví dụ: giấy, bảng viết, tranh,… Dạng thao tác: để đáp ứng yêu cầu viết chữ, bút gắn với phần thao tác vng góc có khả tịnh tiến với mặt phẳng chữ để nhấc bút chuyển nét b) Phân tích yêu cầu vị trí Vị trí mặt phẳng viết nằm vùng làm việc robot c) Yêu cầu hướng khâu thao tác Bút viết có hướng thẳng đứng, vng góc với mặt phẳng viết bàn làm việc d) Yêu cầu vận tốc gia tốc thao tác Vận tốc gia tốc đơn vị thao tác cần đảm bảo không làm biến dạng bề mặt làm việc phụ thuộc vào vật liệu bút vẽ NHÓM 14 Tính tốn, thiết kế robot ứng dụng viết chữ e) Yêu cầu không gian thao tác Không gian thao tác robot thiết kế với diện tích bề mặt kích thước tờ giấy A3 297x420x50 mm3 Mặt phẳng thao tác cần đảm bảo yêu cầu phẳng, nhẵn giữ màu mực sau viết Hình 1-2 Khơng gian làm việc robot 1.3 Xác định đặc trưng kỹ thuật a) Số bậc tự cần thiết Để thực viết chữ, cần tối thiểu chuyển động độc lập mặt phẳng theo phương x y Ngoài sau lần kết thúc nét vẽ, cần nhấc bút lên, hạ bút xuống theo phương z Vì số bậc tự cần thiết robot bậc b) Vùng làm việc robot Đảm bảo robot làm việc không gian 297x420x50 mm3 c) Yêu cầu tải trọng Đặc tính robot viết chữ nên không yêu cầu tải trọng lớn Để đảm bảo chuyển động thực tế lực cản bề mặt làm việc với đầu bút viết yêu cầu tín toán cụ thể phần sau 1.4 Các phương án thiết kế cấu trúc robot a) Các phương án thiết kế: Để tạo nét chữ có hình dạng phức tạp đường cong, thẳng nét chữ kí khơng theo tiêu chuẩn ta cần Robot có hai bậc tự bao quát điểm mặt phẳng, khâu cuối kẹp bút đảm bảo cho vuông góc với bàn máy Để nét chữ khơng bị nguệch ngoạc ta chuyển sang chữ khác ta cần thêm bậc tự giúp bút chuyển động lên xuống Vì dựa vào yếu tố trên, ta thiết kế Robot bậc tự đảm bảo u cầu NHĨM 14 Tính toán, thiết kế robot ứng dụng viết chữ Phương án 1: Robot có bậc tự gồm khâu quay Khâu quay quanh trục Z Khâu quay quanh trục Z khâu chuyển động tịnh tiến dọc trục Z Khâu đảm bảo bao quát điểm bề mặt độ với Khâu đảm bảo việc lên xuống bút tránh tình trạng nhịe mực Hình 1-3 Robot với cấu trúc RRT • • Ưu điểm: - Đảm bảo yêu cầu cần thiết - Tính linh hoạt cao Nhược điểm: - Robot sử dụng khâu quay liên tiếp dễ dẫn đến có góc chết - Chiểm diện tích lớn cồng kềnh với khâu xoay Phương án 2: Robot gồm Khâu tịnh tiến theo phương ngang, khâu xoay quanh trục Z, khâu Về mặt cấu tạo giống với phương án Đảm bảo bút nhấc khỏi mặt giấy cần dừng nét với tới điểm nhờ khâu NHĨM 14 10 Tính tốn, thiết kế robot ứng dụng viết chữ Hình 1-4 Robot với cấu trúc TRR • • Ưu điểm: - Tính linh hoạt tốt - Đảm bảo yếu tố cần thiết đề Nhược điểm: - Chiếm dụng nhiều không gian diện tích - Dễ xảy góc chết khâu - Không đảm bảo tính liên tục Phương án 3: Robot gồm khâu tịnh tiến theo trục X, Y, Z Khâu 1, tịnh tiến theo phương X, Y đảm bảo cho bút bao quát toàn khổ giấy Khâu mang bút, lên xuống vng góc với bàn máy NHĨM 14 10 53 Tính tốn, thiết kế robot ứng dụng viết chữ c) Kết mô động học Robot Bằng cách kết hợp quỹ đạo chuyển động dạng đường thẳng dạng cung trịn, robot mơ viết chữ để tạo thành từ “ROBOT” sau đây: Hình 7-18 Robot viết chữ “ROBOT” theo quỹ đạo đường thẳng cung tròn kết hợp NHĨM 14 53 54 Tính tốn, thiết kế robot ứng dụng viết chữ PHỤ LỤC Code không gian làm việc %clear all clc %variable syms d1 d2 d3 a1 = 50; T01 = [1 0 a1; 0 0; -1 d1; 0 1]; T12 = [0 -1 0; 0 0; -1 d2; 0 1]; T23 = [1 0 0; 0; 0 d3; 0 1]; T03 = T01 * T12 * T23 x = T03(1,4) y = T03(2,4) z = T03(3,4) d1 = linspace(0, 420,50); d2 = linspace(0, 300,50); d3 = linspace(0, 50,50); [q1, q2, q3] = ndgrid(d1, d2, d3); xP = a1 - q3; yP = q2; zP = q1; plot3(xP(:),yP(:),zP(:),'.'); hold on xlabel('x(mm)'); ylabel('y(mm)'); zlabel('z(mm)'); grid on Code thiết kế quỹ đạo clear clc %Bai toan %thiet ke quy dao di tu A(xa,ya,za) den B(xb,yb,zb) %trong do: thoi gian pha tang toc va giam toc la 1(s) %Khai bao cac bien he thong: syms xa ya za xb yb zb tf assume(xa,'real'); assume(ya,'real'); assume(za,'real'); assume(xb,'real'); assume(yb,'real'); assume(zb,'real'); assume(tf,'real'); assume(tf>2); %Nhap toa A B NHÓM 14 54 55 Tính tốn, thiết kế robot ứng dụng viết chữ diemA = [10 150 diemB = [10 150 diemC = [10 150 diemD = [10 100 xa = diemA(1); ya = diemA(2); za = diemA(3); xb = diemB(1); yb = diemB(2); zb = diemB(3); xc = diemC(1); yc = diemC(2); zc = diemC(3); xd = diemD(1); yd = diemD(2); zd = diemD(3); 350]; 325]; 325]; 350]; %Khai bao cac khoang thoi gian t0 = 0; te = 5; t1 = t0: 0.01: te; t2 = t0: 0.01: te; % -%toa diem cong tac tu A den B theo duong thang if xa~=xb a0 = xa; a1 = 0; a2 = 3*(xb-xa)/(te-t0)^2; a3 = -2*(xb-xa)/(te-t0)^3; x = a0 + a1*t1 + a2*t1.^2 + a3*t1.^3; y = x*(yb-ya)/(xb-xa)-xa*(yb-ya)/(xb-xa)+ya; z = x*(zb-za)/(xb-xa)-xa*(zb-za)/(xb-xa)+za; vx = a1 + 2*a2*t1 + 3*a3*t1.^2; vy = vx.*(yb-ya)/(xb-xa); vz = vx.*(zb-za)/(xb-xa); ax = 2*a2 + 6*a3*t1; ay = ax.*(yb-ya)/(xb-xa); az = ax.*(zb-za)/(xb-xa); elseif ya~=yb a0 = ya; a1 = 0; a2 = 3*(yb-ya)/(te-t0)^2; a3 = -2*(yb-ya)/(te-t0)^3; y = a0 + a1*t1 + a2*t1.^2 + a3*t1.^3; x = y*(xb-xa)/(yb-ya)-ya*(xb-xa)/(yb-ya)+xa; z = (y-ya)*(zb-za)/(yb-ya)+za; vy = a1 + 2*a2*t1 + 3*a3*t1.^2; vx = vy.*(xb-xa)/(yb-ya); vz = vy.*(zb-za)/(yb-ya); ay = 2*a2 + 6*a3*t1; ax = ay.*(xb-xa)/(yb-ya); az = ay.*(zb-za)/(yb-ya); elseif za~=zb a0 = za; a1 = 0; a2 = 3*(zb-za)/(te-t0)^2; a3 = -2*(zb-za)/(te-t0)^3; z = a0 + a1*t1 + a2*t1.^2 + a3*t1.^3; x = (z-za)*(xb-xa)/(zb-za)+xa; NHĨM 14 55 56 Tính toán, thiết kế robot ứng dụng viết chữ y = (z-za)*(yb-ya)/(zb-za)+ya; vz = a1 + 2*a2*t1 + 3*a3*t1.^2; vx = vz.*(xb-xa)/(zb-za); vy = vz.*(yb-ya)/(zb-za); az = 2*a2 + 6*a3*t1; ax = az.*(xb-xa)/(zb-za); ay = az.*(yb-ya)/(zb-za); end plot(t1, x,'r-'),grid on,xlabel('T(s)'), ylabel('toa (mm)') hold on plot(t1, y,'g-'),grid on,xlabel('T(s)') hold on plot(t1, z,'b-'),grid on,xlabel('T(s)') legend('x', 'y', 'z') pause hold off plot3(x,y,z,'.b'), grid on, hold on, xlabel('x(mm)'), ylabel('y(mm)'), zlabel('z(mm)') pause hold off %van toc plot(t1, vx,'r-'),grid on,xlabel('T(s)'),ylabel('v(mm/s)') hold on plot(t1, vy,'g-'),grid on,xlabel('T(s)') hold on plot(t1, vz,'b-'),grid on,xlabel('T(s)') legend('vx', 'vy', 'vz') pause hold off %gia toc plot(t1, ax,'r-'),grid on,xlabel('T(s)'),ylabel('a(mm/s^2)') hold on plot(t1, ay,'g-'),grid on,xlabel('T(s)') hold on plot(t1, az,'b-'),grid on,xlabel('T(s)') legend('ax', 'ay', 'az') pause hold off a1 = 50; q1 = z; q2 = y; q3 = a1 - x; plot(t1,q1,'b-'),grid on,xlabel('T[s]') hold on plot(t1,q2,'r-'),grid on,xlabel('T[s]') hold on plot(t1,q3,'g-'),grid on,xlabel('T[s]'),ylabel('q (mm)') legend('q1', 'q2', 'q3') pause hold off % -%toa diem cong tac tu C den D theo nua cung tron NHÓM 14 56 57 Tính tốn, thiết kế robot ứng dụng viết chữ R = abs(yc-yd)/2; phi0 = pi/2; phie = -pi/2; ap0 = phi0; ap1 = 0; ap2 = 3*(phie-phi0)/(te-t0)^2; ap3 = -2*(phie-phi0)/(te-t0)^3; phi = ap0 + ap1*t2 + ap2*t2.^2 + ap3*t2.^3; xp = xc + 0*t2; yp = yd + (yc-yd)/2 + R*sin(phi); zp = zc - R*cos(phi); wp = ap1 + 2*ap2*t2 + 3*ap3*t2.^2; plot(t2, xp,'r-'),grid on,xlabel('T(s)'), ylabel('toa (mm)') hold on plot(t2, yp,'g-'),grid on,xlabel('T(s)') hold on plot(t2, zp,'b-'),grid on,xlabel('T(s)') legend('x', 'y', 'z') pause hold off plot3(xp,yp,zp,'.b'), grid on, hold on, xlabel('x(mm)'), ylabel('y(mm)'), zlabel('z(mm)') pause hold off a1 = 50; qp1 = zp; qp2 = yp; qp3 = a1 - xp; plot(t2,qp1,'b-'),grid on,xlabel('T[s]') hold on plot(t2,qp2,'r-'),grid on,xlabel('T[s]') hold on plot(t2,qp3,'g-'),grid on,xlabel('T[s]'),ylabel('q (mm)') legend('q1', 'q2', 'q3') pause hold off Code hàm mơ hình điều khiển Simulink Khối QUY DAO DAT function out = quydao(time) t = time; qd = zeros(3, 1); dqd = zeros(3, 1); q = zeros(9, 1); %vi tri: q(1:3), van toc: q(4:6), giatoc: q(7:9) diemA = [10 100 350]; diemB = [10 50 300]; xa = diemA(1); ya = diemA(2); za = diemA(3); xb = diemB(1); yb = diemB(2); zb = diemB(3); t0 = 0; NHĨM 14 57 58 Tính tốn, thiết kế robot ứng dụng viết chữ te = 5; a0 a1 a2 a3 = = = = za; -2; 3*(zb-za)/(te-t0)^2; -2*(zb-za)/(te-t0)^3; z = a0 q(3) = q(1) = q(2) = + a1*t + a2*t^2 + a3*t^3; z; (z-za)*(xb-xa)/(zb-za)+xa; (z-za)*(yb-ya)/(zb-za)+ya; vz = q(6) q(4) q(5) a1 + 2*a2*t + 3*a3*t^2; = vz; = vz.*(xb-xa)/(zb-za); = vz.*(yb-ya)/(zb-za); az = q(9) q(7) q(8) 2*a2 + 6*a3*t; = az; = az.*(xb-xa)/(zb-za); = az.*(yb-ya)/(zb-za); %vi tri, van toc diem cong tac: rE = q(1:3); vE=[q(4);q(5);q(6)]; %dong hoc nguoc: %toa diem cong tac x = rE(1); y = rE(2); z = rE(3); %dong hoc nguoc vi tri qd(1) = z; qd(2) = y; qd(3) = 50 - x; %dong hoc nguoc van toc dqd(1) = q(6); dqd(2) = q(5); dqd(3) = -q(4); out = [qd; dqd]; Khối PHAN TU PHI TUYEN function U = fcn(in) U = zeros(3,1); q = in(1:3); dq = in(4:6); ddq = in(7:9); %cac thong so khau: m1 = 2; m2 = 2; m3 = 2; a1 = 50; L1 = 210; L2 = 150; g = 9.81; NHĨM 14 58 59 Tính toán, thiết kế robot ứng dụng viết chữ %ma m11 m12 m13 m21 m22 m23 m31 m32 m33 M = tran khoi luong: = m1+m2+m3; = 0; = 0; = 0; = m2+m3; = 0; = 0; = 0; = m3; [m11 m12 m13; m21 m22 m23; m31 m32 m33]; %ma tran Coriolis: C = [0 0; 0 0; 0 0]; %ma tran G: G = [0; 0; -m3*g]; U = M*ddq + C*dq + G; Khối ROBOT function ddq = fcn(in) q = in(1:3); dq = in(4:6); U = in(7:9); %cac thong so khau: m1 = 2; m2 = 2; m3 = 2; a1 = 50; L1 = 210; L2 = 150; g = 9.81; %ma m11 m12 m13 m21 m22 m23 m31 m32 m33 M = tran khoi luong: = m1+m2+m3; = 0; = 0; = 0; = m2+m3; = 0; = 0; = 0; = m3; [m11 m12 m13; m21 m22 m23; m31 m32 m33]; %ma tran Coriolis: C = [0 0; 0 0; 0 0]; %ma tran G: G = [0; 0; -m3*g]; %phuong trinh vi phan chuyen dong: ddq = inv(M)*(U - C*dq - G); NHÓM 14 59 60 Tính tốn, thiết kế robot ứng dụng viết chữ Một số thiết lập GUI function q1_slider_Callback(hObject, eventdata, handles) % hObject handle to q1_slider (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider ModelName = 'robot'; %goi mo hinh simulink global var; set_param('robot', 'StopTime', 'inf'); q1 = get(handles.q1_slider, 'value'); %lay gia tri tu slider set(handles.q1_text, 'string', num2str(q1)); %hien thi tu slider sang edit q2 = get(handles.q2_slider, 'value'); set(handles.q2_text, 'string', num2str(q2)); q3 = get(handles.q3_slider, 'value'); set(handles.q3_text, 'string', num2str(q3)); set_param([ModelName '/Slider Gain'],'Gain',num2str(q1)) %lay thong so tu slider gain simulink set_param([ModelName '/Slider Gain1'],'Gain',num2str(q2)) set_param([ModelName '/Slider Gain2'],'Gain',num2str(q3)) %ma x = y = z = tran bien doi toa 50 - q3; q2; q1; %dua toa set(handles.x, set(handles.y, set(handles.z, x,y,z cua 'string', 'string', 'string', EE num2str(x)); num2str(y)); num2str(z)); function pushbutton2_Callback(hObject, eventdata, handles) % hObject handle to pushbutton2 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) ModelName = 'robot'; global var; set_param('robot', 'StopTime', 'inf'); %Manual Switch chuyen sang Kinematic set_param('robot/Manual Switch','sw','1'); set_param('robot/Manual Switch1','sw','1'); set_param('robot/Manual Switch2','sw','1'); xE = str2double(get(handles.x,'string')); %lay gia tri toa diem cuoi yE = str2double(get(handles.y,'string')); zE = str2double(get(handles.z,'string')); NHÓM 14 60 61 Tính tốn, thiết kế robot ứng dụng viết chữ %dong hoc nguoc a1 = 50; q1_inv = zE; q2_inv = yE; q3_inv = a1 - xE; guidata(hObject,handles); set(handles.q1_text,'string',num2str(q1_inv)); set_param([ModelName '/Slider Gain'],'Gain',num2str(q1_inv)); set(handles.q1_slider, 'value', q1_inv); set(handles.q2_text,'string',num2str(q2_inv)); set_param([ModelName '/Slider Gain1'],'Gain',num2str(q2_inv)); set(handles.q2_slider, 'value', q2_inv); set(handles.q3_text,'string',num2str(q3_inv)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(q3_inv)); set(handles.q3_slider, 'value', q3_inv); function q1_text_Callback(hObject, eventdata, handles) % hObject handle to q1_text (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of q1_text as text % str2double(get(hObject,'String')) returns contents of q1_text as a double ModelName = 'robot'; global var; set_param('robot', 'StopTime', 'inf'); q1 = get(handles.q1_text, 'string'); set(handles.q1_slider, 'value', str2double(q1)); q2 = get(handles.q2_text, 'string'); set(handles.q2_slider, 'value', str2double(q2)); q3 = get(handles.q3_text, 'string'); set(handles.q3_slider, 'value', str2double(q3)); set_param([ModelName '/Slider Gain'],'Gain', q1) %lay thong so tu slider gain simulink set_param([ModelName '/Slider Gain1'],'Gain',q2) set_param([ModelName '/Slider Gain2'],'Gain',q3) %ma x = y = z = tran bien doi toa 50 - q3; q2; q1; %dua toa set(handles.x, set(handles.y, set(handles.z, x,y,z cua 'string', 'string', 'string', E num2str(x)); num2str(y)); num2str(z)); function home_button_Callback(hObject, eventdata, handles) % hObject handle to home_button (see GCBO) NHĨM 14 61 62 Tính tốn, thiết kế robot ứng dụng viết chữ % eventdata % handles reserved - to be defined in a future version of MATLAB structure with handles and user data (see GUIDATA) ModelName = 'robot'; %goi mo hinh simulink global var; set_param('robot', 'StopTime', 'inf'); set_param([ModelName '/quydao'],'Value',num2str(1)); %Manual Switch chuyen sang Kinematic set_param('robot/Manual Switch','sw','1'); set_param('robot/Manual Switch1','sw','1'); set_param('robot/Manual Switch2','sw','1'); q1 = 0; %lay gia tri tu slider set(handles.q1_text, 'string', num2str(q1)); %hien thi tu slider sang edit set(handles.q1_slider, 'value', q1); q2 = 0; set(handles.q2_text, 'string', num2str(q2)); set(handles.q2_slider, 'value', q2); q3 = 0; set(handles.q3_text, 'string', num2str(q3)); set(handles.q3_slider, 'value', q3); set_param([ModelName '/Slider Gain'],'Gain',num2str(q1)) %lay thong so tu slider gain simulink set_param([ModelName '/Slider Gain1'],'Gain',num2str(q2)) set_param([ModelName '/Slider Gain2'],'Gain',num2str(q3)) x = 50 - q3; y = q2; z = q1; %dua toa set(handles.x, set(handles.y, set(handles.z, x,y,z cua 'string', 'string', 'string', E num2str(x)); num2str(y)); num2str(z)); %dua toa set(handles.x, set(handles.y, set(handles.z, x,y,z cua 'string', 'string', 'string', EE num2str(x)); num2str(-y)); num2str(z)); function trajectory_button_Callback(hObject, eventdata, handles) % hObject handle to trajectory_button (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) ModelName = 'robot'; global var; set_param('robot','SimulationCommand','stop'); set_param('robot', 'StopTime', get(handles.tc, 'string')); if get(handles.trajectory, 'value')==1 msgbox('Chon dang quy dao chuyen dong!','ERROR','warn'); end NHÓM 14 62 63 Tính tốn, thiết kế robot ứng dụng viết chữ quydao = get(handles.trajectory, 'value'); %dat cac gia tri input cho Matlab Fcn set_param([ModelName '/xA'],'Value',get(handles.xa, 'string')); set_param([ModelName '/yA'],'Value',get(handles.ya, 'string')); set_param([ModelName '/zA'],'Value',get(handles.za, 'string')); set_param([ModelName '/xB'],'Value',get(handles.xb, 'string')); set_param([ModelName '/yB'],'Value',get(handles.yb, 'string')); set_param([ModelName '/zB'],'Value',get(handles.zb, 'string')); set_param([ModelName '/tc'],'Value',get(handles.tc, 'string')); set_param([ModelName '/quydao'],'Value',num2str(quydao)); %Manual Switch chuyen sang Matlab Fcn set_param('robot/Manual Switch','sw','0'); set_param('robot/Manual Switch1','sw','0'); set_param('robot/Manual Switch2','sw','0'); xa=str2double(get(handles.xa, 'string')); ya=str2double(get(handles.ya, 'string')); za=str2double(get(handles.za, 'string')); xb=str2double(get(handles.xb, 'string')); yb=str2double(get(handles.yb, 'string')); zb=str2double(get(handles.zb, 'string')); tc = str2double(get(handles.tc, 'string')); t = 0:0.01: tc; te = tc; t0 = 0; if quydao==2 %duong thang if xa~=xb a0 = xa; a1 = 0; a2 = 3*(xb-xa)/(te-t0)^2; a3 = -2*(xb-xa)/(te-t0)^3; x = a0 + a1*t + a2*t.^2 + a3*t.^3; y = x*(yb-ya)/(xb-xa)-xa*(yb-ya)/(xb-xa)+ya; z = x*(zb-za)/(xb-xa)-xa*(zb-za)/(xb-xa)+za; elseif ya~=yb a0 = ya; a1 = 0; a2 = 3*(yb-ya)/(te-t0)^2; a3 = -2*(yb-ya)/(te-t0)^3; y = a0 + a1*t + a2*t.^2 + a3*t.^3; x = y*(xb-xa)/(yb-ya)-ya*(xb-xa)/(yb-ya)+xa; z = (y-ya)*(zb-za)/(yb-ya)+za; elseif za~=zb a0 = za; a1 = 0; a2 = 3*(zb-za)/(te-t0)^2; a3 = -2*(zb-za)/(te-t0)^3; z = a0 + a1*t + a2*t.^2 + a3*t.^3; x = (z-za)*(xb-xa)/(zb-za)+xa; y = (z-za)*(yb-ya)/(zb-za)+ya; end elseif quydao==3 %cung tron NHÓM 14 63 64 Tính tốn, thiết kế robot ứng dụng viết chữ if ya==yb if za>zb phi0 = pi; phie = 0; else phi0 = 0; phie = -pi; end R = abs(za-zb)/2; ap0 = phi0; ap1 = 0; ap2 = 3*(phie-phi0)/(te-t0)^2; ap3 = -2*(phie-phi0)/(te-t0)^3; phi = ap0 + ap1*t + ap2*t.^2 + ap3*t.^3; x = xa + 0*t; y = ya + R*sin(phi); z = (za+zb)/2 - R*cos(phi) elseif za==zb R = abs(ya-yb)/2; phi0 = pi/2; phie = -pi/2; ap0 = phi0; ap1 = 0; ap2 = 3*(phie-phi0)/(te-t0)^2; ap3 = -2*(phie-phi0)/(te-t0)^3; phi = ap0 + ap1*t + ap2*t.^2 + ap3*t.^3; x = xa + 0*t; y = yb + (ya-yb)/2 + R*sin(phi); z = za - R*cos(phi); end elseif quydao==4 %chu R t1=0:0.01:2; t2=2:0.01:3; t3=3:0.01:5; t4=5:0.01:6; t5=6:0.01:7; %chu O t6=7:0.01:8; t7=8:0.01:9; t8=9:0.01:10; t9=10:0.01:11; t10=11:0.01:12; end axes(handles.axes); plot3(x,y,z,'.b'), grid on, hold on xlabel('x(mm)') ylabel('y(mm)') zlabel('z(mm)') xlim([0 50]) ylim([0 300]) zlim([0 420]) Code thiết kế quỹ đạo mô GUI function out = fcn(u) out = zeros(3,1); NHÓM 14 64 65 Tính tốn, thiết kế robot ứng dụng viết chữ x = u(1); y = u(2); z = u(3); %Input: xa=u(1); ya=u(2); za=u(3); xb=u(4); yb=u(5); zb=u(6); tc=u(7); quydao=u(9); t=u(10); te = tc; t0 = 0; %kiem tra dang quy dao: switch quydao case %duong thang if xa~=xb a0 = xa; a1 = 0; a2 = 3*(xb-xa)/(te-t0)^2; a3 = -2*(xb-xa)/(te-t0)^3; x = a0 + a1*t + a2*t^2 + a3*t^3; y = x*(yb-ya)/(xb-xa)-xa*(yb-ya)/(xb-xa)+ya; z = x*(zb-za)/(xb-xa)-xa*(zb-za)/(xb-xa)+za; elseif ya~=yb a0 = ya; a1 = 0; a2 = 3*(yb-ya)/(te-t0)^2; a3 = -2*(yb-ya)/(te-t0)^3; y = a0 + a1*t + a2*t^2 + a3*t^3; x = y*(xb-xa)/(yb-ya)-ya*(xb-xa)/(yb-ya)+xa; z = (y-ya)*(zb-za)/(yb-ya)+za; elseif za~=zb a0 = za; a1 = 0; a2 = 3*(zb-za)/(te-t0)^2; a3 = -2*(zb-za)/(te-t0)^3; z = a0 + a1*t + a2*t^2 + a3*t^3; x = (z-za)*(xb-xa)/(zb-za)+xa; y = (z-za)*(yb-ya)/(zb-za)+ya; end case %cung tron if ya==yb if za>zb phi0 = pi; phie = 0; else phi0 = 0; phie = -pi; end R = abs(za-zb)/2; ap0 = phi0; ap1 = 0; ap2 = 3*(phie-phi0)/(te-t0)^2; ap3 = -2*(phie-phi0)/(te-t0)^3; phi = ap0 + ap1*t + ap2*t^2 + ap3*t^3; y = ya + R*sin(phi); z = (za+zb)/2 - R*cos(phi); x = xa + 0*t; NHĨM 14 65 66 Tính tốn, thiết kế robot ứng dụng viết chữ elseif za==zb R = abs(ya-yb)/2; phi0 = pi/2; phie = -pi/2; ap0 = phi0; ap1 = 0; ap2 = 3*(phie-phi0)/(te-t0)^2; ap3 = -2*(phie-phi0)/(te-t0)^3; phi = ap0 + ap1*t + ap2*t^2 + ap3*t^3; y = yb + (ya-yb)/2 + R*sin(phi); z = za - R*cos(phi); x = xa + 0*t; end end if (quydao~=1)&&((xa~=xb)||(ya~=yb)||(za~=zb)) plot3(x,y,z,'.g'), grid on, hold on xlabel('x(mm)') ylabel('y(mm)') zlabel('z(mm)') xlim([0 50]) ylim([0 300]) zlim([0 420]) end %dong hoc nguoc vi tri out(1) = z; out(2) = y; out(3) = 50-x; end NHĨM 14 66 67 Tính tốn, thiết kế robot ứng dụng viết chữ TÀI LIỆU THAM KHẢO [1] PGS.TS Phan Bùi Khơi - Bài giảng Robotics [2] Nguyễn Quang Hồng – Sách Matlab & Simulink cho kỹ sư [3] Nguyễn Phùng Quang – Matlab Simulink dành cho kỹ sư điều khiển tự động [4] TS Nguyễn Mạnh Tiến – Điều khiển robot công nghiệp [5] PGS.TS Đào Văn Hiệp – Kỹ thuật robot [6] Matlab Help NHÓM 14 67