BÀI TẬP LỚN KT ROBOT: Tìm hiểu robot PlateCrane EX Microplate Handler

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN  BÀI TẬP LỚN Tính toán Robot hình trụ PlateCrane EX Microplate Handler GV Hướng Dẫn PGS TS Nguyễn Phạm Thục Anh Mã Học Phần EE4344 Mã Lớp 129135 Hà Nội, 122021 STT Họ Tên MSSV 1 Lê Duy Huy 20173957 2 Lê Đức Thọ 20174244 3 Nguyễn Duy Phương 20174122 4 Vũ Minh Hiếu 20173854 5 Trần Việt Hưng 20173938 6 Nguyễn Đức Hiền 20173845 7 Vũ Mạnh Trường 20174304 8 Phạm Hữu Thanh 20181759 9 Nguyễn Văn Trụ 20181792 10 Trịnh Văn Hùng 20173932 MỤC LỤC DANH MỤC BẢ.

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN - - BÀI TẬP LỚN Tính tốn Robot hình trụ PlateCrane EX Microplate Handler GV.Hướng Dẫn: PGS.TS Nguyễn Phạm Thục Anh Mã Học Phần: EE4344 Mã Lớp: 129135 STT 10 Họ & Tên Lê Duy Huy Lê Đức Thọ Nguyễn Duy Phương Vũ Minh Hiếu Trần Việt Hưng Nguyễn Đức Hiền Vũ Mạnh Trường Phạm Hữu Thanh Nguyễn Văn Trụ Trịnh Văn Hùng Hà Nội, 12/2021 MSSV 20173957 20174244 20174122 20173854 20173938 20173845 20174304 20181759 20181792 20173932 MỤC LỤC DANH MỤC BẢNG BIỂU DANH MỤC HÌNH VẼ ĐỀ TÀI Giới thiệu Robot .5 Robot PlateCrane EX Ứng dụng công nghiệp Thông số kỹ thuật Hình ảnh vận hành thực tế Động học thuận vị trí .9 Xác định hệ trục Xác định bảng D-H 10 Xác định ma trận 10 Chương trình tính toán sử dụng Matlab 11 Ma Trận Jacoby 13 Ma trận Jacoby 13 Chương trình tính tốn sử dụng Matlab 15 Động học đảo vị trí 17 Lý thuyết 17 Động học đảo Robot PlateCrane EX 17 Thiết kế quỹ đạo chuyển động .19 Cơ sở lý thuyết 19 Thiết kế quỹ đạo cho Robot PlateCrane EX 19 Xây dựng mơ hình động lực học 25 Thiết kế robot PlateCrane EX phần mềm Solid Work 25 Kết nối Solid Work Matlab/Tool box simscape 28 Thiết kế điều khiển 31 Kết nối điều khiển mơ hình động lực học 33 Kết Luận .38 Phụ lục 39 Động học thuận-Ma trận Jacoby 39 Động học đảo 45 Thiết kế quỹ đạo (Chương 5) 45 ĐHBKHN Kỹ thuật Robot- 129135 DANH MỤC BẢNG BIỂU Bảng 1-1 Thông số kỹ thuật Robot .7 Bảng 2-1 Bảng D-H robot PlateCrane EX 10 Bảng 6-1 Thông số PID 32 Nhóm ĐHBKHN Kỹ thuật Robot- 129135 DANH MỤC HÌNH VẼ Hình 1.1 Robot PlateCrane EX ngồi thực tế Hình 1.2 Không gian làm việc robot PlateCrane EX Hình 1.3 Hình ảnh robot PlateCrane EX .8 Hình 2.1 Cách xác định trục Xi trường hợp Hình 2.2 Gán hệ trục tọa độ cho robot PlateCrane EX .10 Hình 2.3 Giao diện tính tốn ban đầu 12 Hình 2.4 Kết tính tốn ma trận T04 12 Hình 3.1 Giao diện tính tốn ban đầu 15 Hình 3.2 Kết tính tốn ma trận Jacoby 16 Hình 5.1 Đồ thị quỹ đạo, vận tốc gia tốc khớp 21 Hình 5.2 Đồ thị quỹ đạo, vận tốc gia tốc khớp 22 Hình 5.3 Đồ thị quỹ đạo, vận tốc gia tốc khớp 23 Hình 5.4 Đồ thị quỹ đạo, vận tốc gia tốc khớp 24 Hình 6.1 Phần đế Robot .25 Hình 6.2 Thanh nối 25 Hình 6.3 Thanh nối 26 Hình 6.4 Thanh nối 26 Hình 6.5 Khâu tác động cuối .27 Hình 6.6 Robot PlateCrane EX sau ráp hồn chỉnh 27 Hình 6.7 Mơ hình động lực học Robot Simulink 29 Hình 6.8 Mơ hình robot Matlab 31 Hình 6.9 Mơ hình điều khiển PID theo không gian khớp 32 Hình 6.10 Bộ PID cho khớp 32 Hình 6.11 Bộ PID cho khớp 32 Hình 6.12 Bộ PID cho khớp 33 Hình 6.13 Bộ PID cho khớp 33 Hình 6.14 Sơ đồ sau kết nối hoàn chỉnh 34 Hình 6.15 Đáp ứng vị trí khớp 35 Hình 6.15 Đáp ứng vị trí khớp 35 Hình 6.15 Đáp ứng vị trí khớp 36 Hình 6.15 Đáp ứng vị trí khớp 36 Nhóm ĐHBKHN Kỹ thuật Robot- 129135 ĐỀ TÀI Tính tốn thiết kế Robot hình trụ PlateCrane EX Microplate Handler Yêu cầu: 1) Giới thiệu Robot nhóm nghiên cứu, ứng dụng cơng nghiệp, kết cấu khí, thơng số kỹ thuật u cầu có hình ảnh clip hoạt động 2) Tính tốn động học thuận vị trí Robot Xây dựng chương trình phần mềm MatLab để nhập liệu, hiển thị kết 3) Tính tốn ma trận Jacoby (thơng qua JH) viết chương trình MatLab 4) Tính tốn động học đảo vị trí Robot 5) Thiết kế quỹ đạo chuyển động cho khớp Robot theo quỹ đạo dạng bậc 6) Xây dựng mơ hình động lực học cho đối tượng ToolBox Simscape/MatLab Nhóm ĐHBKHN Kỹ thuật Robot- 129135 Giới thiệu Robot Robot PlateCrane EX Robot báo cáo robot PlateCrane EX hãng Hudsonrobotics Robot PlateCrane EX robot linh hoạt, thích hợp cho việc tự động hóa vận chuyển vật liệu Nhờ trang bị phần mềm Hudson’s SoftLinx, PlateCrane EX định cấu hình để phục vụ nhiều thiết bị tương thích với nhiều loại khác PlateCrane EX robot tác động tốc độ cao với trục Có thể cấu hình với tối đa 15 xếp chồng lên sử dụng 10 ngăn xếp thay Được thiết kế để dễ sử dụng trì độ tin cậy tính linh hoạt yêu cầu chuyên gia tự động hóa Tất hệ thống PlateCrane kèm với sở sáng tạo cho phép nhiều thiết bị gắn trực tiếp vào cánh tay robot Hệ thống khóa lại với tạo ổn định làm việc hầu hết bề mặt Hình 1.1 Robot PlateCrane EX ngồi thực tế Ứng dụng công nghiệp PlateCrane EX giải pháp lý tưởng để tự động hóa việc xử lý phịng thí nghiệm: ▪ Xếp chồng vận chuyển vi mẫu tiêu chuẩn ▪ Bao gồm phiên đầy đủ phần mềm lập trình SoftLinx V Nhóm ĐHBKHN Kỹ thuật Robot- 129135 ▪ Tấm Crane có sẵn với kẹp tiêu chuẩn kẹp bên cho ứng dụng truy cập ngẫu nhiên Cả hai kẹp quay để làm việc với vị trí lồng ngang dọc ▪ Tất hệ thống PlateCrane hoạt động với có nắp đậy ▪ TấmCrane tiêu chuẩn với ngăn xếp, ngăn xếp chứa 25 có nắp, 30 khơng có nắp ▪ Số lượng mở rộng lên 15 ngăn xếp tối đa ba băng chuyền 10 ngăn xếp ▪ Có sẵn ngăn xếp kiểm soát nhiệt độ tùy chọn, mở rộng dễ vận hành giá phải Thông số kỹ thuật ▪ Số bậc tự do: ▪ Gồm khớp quay, khớp tịnh tiến ▪ Vùng không gian làm việc: Nhóm ĐHBKHN Kỹ thuật Robot- 129135 Hình 1.2 Khơng gian làm việc robot PlateCrane EX Ta có bảng thông số kĩ thuật robot: Bảng 1-1 Thông số kỹ thuật Robot Chuyển động quay 345° Chuyển động chiều ngang cánh tay (khớp 3) 12 - 18 inches từ trung tâm Chuyển động chiều dọc (khớp 2) Từ - 22 inches Chiều cao cân nặng 29 inches; 20.5kg Nhiệt độ hoạt động 15°C to 40°C (59° to 104°F) Truyền thông RS232 Nguồn 115/220 VAC, 50/60 Hz Hình ảnh vận hành thực tế Một vài video vận hành thực tế Robot PlateCrane EX https://bitly.com.vn/54x5dg https://bitly.com.vn/wv35xg https://bitly.com.vn/kxxmcw Nhóm 7 ĐHBKHN Kỹ thuật Robot- 129135 Hình 1.3 Hình ảnh robot PlateCrane EX Nhóm ĐHBKHN Kỹ thuật Robot- 129135 Động học thuận vị trí Xác định hệ trục Theo tài liệu hãng, robot PlateCrane EX có bậc tự Trước xác định bảng Denavit – Hartenberg, ta cần xác định hệ trục tọa độ cho khớp: ▪ Xác định trục Zi: tất khớp robot khớp quay nên Zi trục mà khớp (i+1) quay xung quanh ▪ Xác định trục Xi: + Nếu Zi-1 Zi chéo Xi đường vng góc chung Zi-1 Zi + Nếu Zi-1 Zi song song Xi nằm đường vng góc Zi qua tâm Oi Oi-1 + Nếu Zi-1 cắt Zi, chọn Oi giao điểm Zi-1 Zi, Xi đường vuông góc mặt phẳng chứa Zi-1 Zi điểm Oi Zi-1 Zi Zi Zi-1 Xi Xi-1 Xi Oi-1 Oi Oi Xi Zi-1 Zi Hình 2.1 Cách xác định trục Xi trường hợp Nhóm ĐHBKHN Kỹ thuật Robot- 129135 Hình 6.12 Bộ PID cho khớp Hình 6.13 Bộ PID cho khớp ➢ Thông số mô Matlab: T1 T2 T3 T4 = = = = -90; 0.2; 0.05; 150; %goc quay khop %do di tinh tien %do di tinh tien %goc quay khop (degree) khop (m) khop (m) (degree) %PID kp1 = 50; ki1 = 30; kd1 = 5; %PID kp2 = 100; ki2 = 20; kd2 = 20; %PID kp3 = 400; ki3 = 40; kd3 = 120; %PID kp4 = 100; ki4 = 5; kd4 = 5; Kết nối điều khiển mô hình động lực học Nhóm 33 ĐHBKHN Kỹ thuật Robot- 129135 Tiếp theo, ta thêm điều khiển vào mơ Hình 6.14: Hình 6.14 Sơ đồ sau kết nối hồn chỉnh • Ta sử dụng khối Simulink-PS converter & PS-Simulink converter để chuyển đổi giá trị điều khiển thành giá trị góc quay/tịnh tiến cho khớp nhận tín hiệu phản hồi khớp điều khiển • Vùng SCOPE cho phép ta so sánh giá trị lượng đặt đáp ứng khớp kết đây: Nhóm 34 ĐHBKHN Kỹ thuật Robot- 129135 Hình 6.15 Đáp ứng vị trí khớp Hình 6.16 Đáp ứng vị trí khớp Nhóm 35 ĐHBKHN Kỹ thuật Robot- 129135 Hình 6.17 Đáp ứng vị trí khớp Hình 6.18 Đáp ứng vị trí khớp ✓ Nhận xét: • Sai lệch tĩnh điều khiển cho khớp ≈ • Tuy nhiên độ điều chỉnh lớn cần phải điều chỉnh tham số Kp, Ki,Kd để tối ưu • Mơ thành cơng kết hợp điều khiển PID mơ hình động lực học Simscape/MatLab Nhóm 36 ĐHBKHN Nhóm Kỹ thuật Robot- 129135 37 ĐHBKHN Kỹ thuật Robot- 129135 Kết Luận Đề tài tìm hiểu robot PlateCrane EX, loại robot phổ biến phịng thí nghiệm cơng nghiệp giúp nhóm chúng em hiểu vấn đề môn học Kỹ thuật robot nhóm động học thuận, động học ngược, thiết kế quỹ đạo… ngồi cịn cải thiện việc sử dụng công cụ phần mềm Matlab/Simulink, Solid Work Trong q trình hồn thành báo cáo khơng tránh khỏi sai sót nhóm chúng em mong nhận ý kiến đóng góp bạn để báo cáo hồn thiện Nhóm xin chân thành cảm ơn hướng dẫn tận tình PGS Nguyễn Phạm Thục Anh thời gian qua Nhóm 38 ĐHBKHN Kỹ thuật Robot- 129135 Phụ lục Động học thuận-Ma trận Jacoby ➢ Tính ma trận T04, ma trận Jacoby clc clear all syms c1 s1 c4 s4 d2 d3 d4 %Nhap A1, A2, A3, A4 A1 = [-s1 -c1 0;c1 -s1 0;0 0;0 0 1]; A2 = [1 0 0;0 0;0 -1 d2;0 0 1]; A3 = [1 0 0;0 0;0 -1 d3;0 0 1]; A4 = [c4 -s4 0;s4 c4 0;0 d4;0 0 1]; T34 T24 T14 T04 = = = = simplify(A4) simplify(A3*A4) simplify(A2*A3*A4) simplify(A1*A2*A3*A4) %% Jacoby Matrix %Khop J11 = T04(2,1)*T04(1,4)-T04(1,1)*T04(2,4) J21 = T04(2,2)*T04(1,4)-T04(1,2)*T04(2,4) J31 = T04(2,3)*T04(1,4)-T04(1,3)*T04(2,4) J41 = T04(3,1) %nz J51 = T04(3,2) %oz J61 = T04(3,3) %az %Khop J12 = T14(3,1) %nz J22 = T14(3,2) %oz J32 = T14(3,3) %az J42 = J52 = J62 = %Khop J13 = T24(3,1) J23 = T24(3,2) J33 = T24(3,3) J43 = J53 = J63 = % Khop J14 = T34(2,1)*T34(1,4)-T34(1,1)*T34(2,4) J24 = T34(2,2)*T34(1,4)-T34(1,2)*T34(2,4) J34 = T34(2,3)*T34(1,4)-T34(1,3)*T34(2,4) J44 = T34(3,1) J54 = T34(3,2) J64 = T34(3,3) % JH Jh = [J11 J12 J13 J14 J21 J22 J23 J24 J31 J32 J33 J34 J41 J42 J43 J44 J51 J52 J53 J54 J61 J62 J63 J64]; %R04 R04 = T04(1:3,1:3); R = [R04,zeros(3,3);zeros(3,3),R04]; J = simplify(R*Jh) %ma tran Jacoby Nhóm % ny*px-nx*py % oy*px-ox*py % ay*px-ax*py 39 ĐHBKHN Kỹ thuật Robot- 129135 ➢ Thiết kế giao diện function varargout = giaodien(varargin) % GIAODIEN MATLAB code for giaodien.fig % GIAODIEN, by itself, creates a new GIAODIEN or raises the existing % singleton* % % H = GIAODIEN returns the handle to a new GIAODIEN or the handle to % the existing singleton* % % GIAODIEN('CALLBACK',hObject,eventData,handles, ) calls the local % function named CALLBACK in GIAODIEN.M with the given input arguments % % GIAODIEN('Property','Value', ) creates a new GIAODIEN or raises the % existing singleton* Starting from the left, property value pairs are % applied to the GUI before giaodien_OpeningFcn gets called An % unrecognized property name or invalid value makes property application % stop All inputs are passed to giaodien_OpeningFcn via varargin % % *See GUI Options on GUIDE's Tools menu Choose "GUI allows only one % instance to run (singleton)" % % See also: GUIDE, GUIDATA, GUIHANDLES % Edit the above text to modify the response to help giaodien % Last Modified by GUIDE v2.5 03-Jan-2022 10:01:06 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, 'gui_Singleton', gui_Singleton, 'gui_OpeningFcn', @giaodien_OpeningFcn, 'gui_OutputFcn', @giaodien_OutputFcn, 'gui_LayoutFcn', [] , 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end % End initialization code - DO NOT EDIT % - Executes just before giaodien is made visible function giaodien_OpeningFcn(hObject, eventdata, handles, varargin) % This function has no output args, see OutputFcn % hObject handle to figure Nhóm 40 ĐHBKHN % eventdata % handles % varargin Kỹ thuật Robot- 129135 reserved - to be defined in a future version of MATLAB structure with handles and user data (see GUIDATA) command line arguments to giaodien (see VARARGIN) % Choose default command line output for giaodien handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes giaodien wait for user response (see UIRESUME) % uiwait(handles.figure1); % - Outputs from this function are returned to the command line function varargout = giaodien_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; % - Executes on button press in Tinh function Tinh_Callback(hObject, eventdata, handles) % Get Data theta1 = str2num(get(handles.nhap_theta1,'string')); d2 = str2num(get(handles.nhap_d2,'string')); d3 = str2num(get(handles.nhap_d3,'string')); theta4 = str2num(get(handles.nhap_theta4,'string')); %%Tinh ma tran T04 %1.1 T4011 = cosd(theta1)*sind(theta4)-cosd(theta4)*sind(theta1); set(handles.T4011,'string',num2str(T4011)); %1.2 T4012 = cosd(theta1)*cosd(theta4)+sind(theta1)*sind(theta4); set(handles.T4012,'string',num2str(T4012)); %1.3 T4013 = 0; set(handles.T4013,'string',num2str(T4013)); %1.4 T4014 = -d3*cosd(theta1); set(handles.T4014,'string',num2str(T4014)); %2.1 T4021 = cosd(theta1)*cosd(theta4)+sind(theta1)*sind(theta4); set(handles.T4021,'string',num2str(T4021)); %2.2 T4022 = cosd(theta4)*sind(theta1)-sind(theta4)*cosd(theta1); set(handles.T4022,'string',num2str(T4022)); %2.3 T4023 = 0; set(handles.T4023,'string',num2str(T4023)); %2.4 T4024 = -d3*sind(theta1); set(handles.T4024,'string',num2str(T4024)); %3.1 T4031 = 0; set(handles.T4031,'string',num2str(T4031)); %3.2 T4032 = 0; set(handles.T4032,'string',num2str(T4032)); %3.3 T4033 = -1; Nhóm 41 ĐHBKHN Kỹ thuật Robot- 129135 set(handles.T4033,'string',num2str(T4033)); %3.4 T4034 = d2-139; set(handles.T4034,'string',num2str(T4034)); %4.1 T4041 = 0; set(handles.T4041,'string',num2str(T4041)); %4.2 T4042 = 0; set(handles.T4042,'string',num2str(T4042)); %4.3 T4043 = 0; set(handles.T4043,'string',num2str(T4043)); %4.4 T4044 = 1; set(handles.T4044,'string',num2str(T4044)); %% %Jacoby JH11= d3*sind(theta1)*(cosd(theta1)*sind(theta4)sind(theta1)*cosd(theta4))d3*cosd(theta1)*(cosd(theta1)*cosd(theta4)+sind(theta1)*sind(theta4)); JH21= d3*sind(theta1)*(cosd(theta1)*cosd(theta4)+sind(theta1)*sind(theta4))+d 3*cosd(theta1)*(cosd(theta1)*sind(theta4)-sind(theta1)*cosd(theta4)); JH31=0; JH41=0; JH51=0; JH61=-1; JH12=0; JH22=0; JH32=-1; JH42=0; JH52=0; JH62=0; JH13=-sind(theta4); JH23=-cosd(theta4); JH33=0; JH43=0; JH53=0; JH63=0; JH14=0; JH24=0; JH34=0; JH44=0; JH54=0; JH64=1; JH = [JH11 JH12 JH13 JH14; JH21 JH22 JH23 JH24; JH31 JH32 JH33 JH34; JH41 JH42 JH43 JH44; JH51 JH52 JH53 JH54; JH61 JH62 JH63 JH64]; R66=[T4011 T4012 T4013 0 0; T4021 T4022 T4023 0 0; T4031 T4032 T4033 0 0; 0 T4011 T4012 T4013; 0 T4021 T4022 T4023; 0 T4031 T4032 T4033]; J=R66*JH; Nhóm 42 ĐHBKHN Kỹ thuật Robot- 129135 set(handles.a11,'String',num2str(J(1,1))); set(handles.a12,'String',num2str(J(1,2))); set(handles.a13,'String',num2str(J(1,3))); set(handles.a14,'String',num2str(J(1,4))); set(handles.a21,'String',num2str(J(2,1))); set(handles.a22,'String',num2str(J(2,2))); set(handles.a23,'String',num2str(J(2,3))); set(handles.a24,'String',num2str(J(2,4))); set(handles.a31,'String',num2str(J(3,1))); set(handles.a32,'String',num2str(J(3,2))); set(handles.a33,'String',num2str(J(3,3))); set(handles.a34,'String',num2str(J(3,4))); set(handles.a41,'String',num2str(J(4,1))); set(handles.a42,'String',num2str(J(4,2))); set(handles.a43,'String',num2str(J(4,3))); set(handles.a44,'String',num2str(J(4,4))); set(handles.a51,'String',num2str(J(5,1))); set(handles.a52,'String',num2str(J(5,2))); set(handles.a53,'String',num2str(J(5,3))); set(handles.a54,'String',num2str(J(5,4))); set(handles.a61,'String',num2str(J(6,1))); set(handles.a62,'String',num2str(J(6,2))); set(handles.a63,'String',num2str(J(6,3))); set(handles.a64,'String',num2str(J(6,4))); % - Executes on button press in Xoa function Xoa_Callback(hObject, eventdata, handles) set(handles.nhap_theta1,'string',' '); set(handles.nhap_d2,'string',' '); set(handles.nhap_d3,'string',' '); set(handles.nhap_theta4,'string',' '); set(handles.T4011,'string',' '); set(handles.T4012,'string',' '); set(handles.T4013,'string',' '); set(handles.T4014,'string',' '); set(handles.T4021,'string',' '); set(handles.T4022,'string',' '); set(handles.T4023,'string',' '); set(handles.T4024,'string',' '); set(handles.T4031,'string',' '); set(handles.T4032,'string',' '); set(handles.T4033,'string',' '); set(handles.T4034,'string',' '); set(handles.T4041,'string',' '); set(handles.T4042,'string',' '); set(handles.T4043,'string',' '); set(handles.T4044,'string',' '); %Jacoby set(handles.a11,'String',' '); set(handles.a12,'String',' '); set(handles.a13,'String',' '); set(handles.a14,'String',' '); set(handles.a21,'String',' '); Nhóm 43 ĐHBKHN Kỹ thuật Robot- 129135 set(handles.a22,'String',' set(handles.a23,'String',' set(handles.a24,'String',' set(handles.a31,'String',' set(handles.a32,'String',' set(handles.a33,'String',' set(handles.a34,'String',' set(handles.a41,'String',' set(handles.a42,'String',' set(handles.a43,'String',' set(handles.a44,'String',' set(handles.a51,'String',' set(handles.a52,'String',' set(handles.a53,'String',' set(handles.a54,'String',' set(handles.a61,'String',' set(handles.a62,'String',' set(handles.a63,'String',' set(handles.a64,'String',' '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); function nhap_theta1_Callback(hObject, eventdata, handles) theta1 = str2num(get(handles.nhap_theta1,'string')); if (theta1 > 345) theta1 = 345; end if (theta1 < 0) theta1 = 0; end set(handles.nhap_theta1,'string', num2str(theta1)); function nhap_theta1_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_d2_Callback(hObject, eventdata, handles) d2 = str2num(get(handles.nhap_d2,'string')); if (d2 > 460) d2 = 460; end if (d2 < 0) d2 = 0; end set(handles.nhap_d2,'string',num2str(d2)); function nhap_d2_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_d3_Callback(hObject, eventdata, handles) d3 = str2num(get(handles.nhap_d3,'string')); if (d3 > 459) d3 = 459; end if (d3 < 301) d3 = 301; end Nhóm 44 ĐHBKHN Kỹ thuật Robot- 129135 set(handles.nhap_d3,'string',num2str(d3)); function nhap_d3_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta4_Callback(hObject, eventdata, handles) theta4 = str2num(get(handles.nhap_theta4,'string')); if (theta4 > 345) theta4 = 345; end if (theta4 < 0) theta4 = 0; end set(handles.nhap_theta4,'string',num2str(theta4)); % - Executes during object creation, after setting all properties function nhap_theta4_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end Động học đảo -1 ➢ Tính ma trận A1 , Tính vế phải VF1 clc clear all syms c1 s1 c4 s4 d2 d3 d4 %Nhap A1, A2, A3, A4 A1 = [-s1 -c1 0;c1 -s1 0;0 0;0 0 1]; A2 = [1 0 0;0 0;0 -1 d2;0 0 1]; A3 = [1 0 0;0 0;0 -1 d3;0 0 1]; A4 = [c4 -s4 0;s4 c4 0;0 d4;0 0 1]; % A1^-1 A1_1 = inv(A1) % VF1 VF1 = A2*A3*A4 Thiết kế quỹ đạo (Chương 5) ➢ Vẽ quỹ đạo, vận tốc, gia tốc khớp clc clear all t = [0:0.001:2]; tc = 2; % thoi gian chuyen dong moi khop 2s %diem A [40,250,350,120] theta1A = 40; r2A = 250; r3A = 350; theta4A = 120; %diem B [80,150,400,20] Nhóm 45 ĐHBKHN Kỹ thuật Robot- 129135 theta1B = 80; r2B = 150; r3B = 400; theta4B = 20; %% figure(1) % khop q1_1 = theta1A + 3*(theta1B - theta1A)/(tc^2).*(t.^2) - 2*(theta1Btheta1A)/(tc^3).*(t.^3); q1_2 = 2*(3*(theta1B - theta1A)/(tc^2)).*t - 3*2*(theta1B theta1A)/(tc^3).*(t.^2); q1_3 = 2*(3*(theta1B - theta1A)/(tc^2)) - 2*3*2*(theta1B theta1A)/(tc^3).*t; subplot(3,1,1) plot(t,q1_1,'LineWidth',2); grid on; subplot(3,1,2) plot(t,q1_2,'LineWidth',2); grid on; subplot(3,1,3) plot(t,q1_3,'LineWidth',2); grid on; %% figure(2) %khop q2_1 = r2A + 3*(r2B - r2A)/(tc^2).*(t.^2) - 2*(r2B r2A)/(tc^3).*(t.^3); q2_2 = 2*(3*(r2B - r2A)/(tc^2)).*t - 3*2*(r2B - r2A)/(tc^3).*(t.^2); q2_3 = 2*(3*(r2B - r2A)/(tc^2)) - 2*3*2*(r2B - r2A)/(tc^3).*t; subplot(3,1,1) plot(t,q2_1,'LineWidth',2); grid on; subplot(3,1,2) plot(t,q2_2,'LineWidth',2); grid on; subplot(3,1,3) plot(t,q2_3,'LineWidth',2); grid on; %% figure(3) %khop q3_1 = r3A + 3*(r3B - r3A)/(tc^2).* (t.^2) - 2*(r3B r3A)/(tc^3).*(t.^3); q3_2 = 2*(3*(r3B - r3A)/(tc^2)).*t - 3*2*(r3B - r3A)/(tc^3).*(t.^2); q3_3 = 2*(3*(r3B - r3A)/(tc^2)) - 2*3*2*(r3B - r3A)/(tc^3).*t; subplot(3,1,1) plot(t,q3_1,'LineWidth',2); grid on; subplot(3,1,2) plot(t,q3_2,'LineWidth',2); grid on; subplot(3,1,3) plot(t,q3_3,'LineWidth',2); grid on; %% figure(4) %khop q4_1 = theta4A + 3*(theta4B - theta4A)/(tc^2).*(t.^2) - 2*(theta4B theta4A)/(tc^3).*(t.^3); q4_2 = 2*(3*(theta4B - theta4A)/(tc^2)).*t - 3*2*(theta4B theta4A)/(tc^3).*(t.^2); q4_3 = 2*(3*(theta4B - theta4A)/(tc^2)) - 2*3*2*(theta4B theta4A)/(tc^3).*t; subplot(3,1,1) plot(t,q4_1,'LineWidth',2); grid on; subplot(3,1,2) Nhóm 46 ĐHBKHN Kỹ thuật Robot- 129135 plot(t,q4_2,'LineWidth',2); grid on; subplot(3,1,3) plot(t,q4_3,'LineWidth',2); grid on; Nhóm 47 ... 350 a = q3 (t ) = 350 + 37. 5t − 12.5t 13    • 3(400 − 350) → q (t ) = 75 t − 37. 5t a23 = = 37. 5 22  ••  q (t ) = 75 − 75 t 2(400 − 350) = − 12.5 a33 = −  23 Nhóm 22 ĐHBKHN Kỹ thuật... = 120 a = q4 (t ) = 120 − 75 t + 25t 14  •   3(20 − 120) → q (t ) = −150t + 75 t a24 = = − 75 22  ••  2(20 − 120) q (t ) = −150 + 150t = 25 a34 = −  23 Nhóm 23 ĐHBKHN Kỹ thuật Robot-... Simscape/MatLab Nhóm 36 ĐHBKHN Nhóm Kỹ thuật Robot- 129135 37 ĐHBKHN Kỹ thuật Robot- 129135 Kết Luận Đề tài tìm hiểu robot PlateCrane EX, loại robot phổ biến phịng thí nghiệm cơng nghiệp giúp nhóm chúng