BÀI tập lớn kỹ THUẬT ROBOT TÍNH TOÁN THIẾT kế ROBOT MH3BM

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI BÀI TẬP LỚN KỸ THUẬT ROBOT TÍNH TỐN THIẾT KẾ ROBOT MH3BM Họ tên Mã số sinh viên Nguyễn Đức Thuận 20174248 Nguyễn Việt Hoàng 20173897 Nguyễn Đức Minh 20181651 Lê Văn Dương 20181434 Phạm Tiến Đạt 20181389 Nguyễn Văn Dương 20181441 Cao Thanh Tùng 20181819 Trần Minh Thân 20181751 Nguyễn Trọng Hoàng 20173914 10 Hoàng Ngọc Dũng 20181423 Giảng viên hướng dẫn: PGS.TS Nguyễn Phạm Thục Anh Bộ môn: Viện: Tự động hóa Cơng nghiệp Điện HÀ NỘI, 1/2022 MỤC LỤC CHƯƠNG GIỚI THIỆU ROBOT MH3BM 1.1 Lịch sử hình thành Robot 1.2 Robot MH3BM 1.3 Ứng dụng công nghiệp 1.4 Thông số kĩ thuật 1.5 Hình ảnh video làm việc thực tế CHƯƠNG ĐỘNG HỌC THUẬN VỊ TRÍ 2.1 Xác định khớp, nối khai báo hệ trục 2.2 Xác định bảng D-H 10 2.3 Xác định ma trận 11 2.4 Xây dựng giao diện tính tốn 12 CHƯƠNG MA TRẬN JACOBY 14 3.1 Ma trận Jacoby 14 3.2 Xây dựng giao diện tính tốn 17 CHƯƠNG ĐỘNG HỌC ĐẢO VỊ TRÍ 19 4.1 Động học đảo robot MH3BM 19 CHƯƠNG THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 22 5.1 Lý thuyết 22 5.2 Thiết kế quỹ đạo 22 CHƯƠNG XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG TRÊN TOOLBOX SIMSCAPE/MATLAB 30 6.1 Thiết kế Solid Work 30 6.2 Kết nối Solid Work Matlab 33 6.3 Thiết kế điều khiển PID 34 CHƯƠNG PHỤ LỤC 41 7.1 Giao diện động học thuận 41 7.2 Giao diện tính ma trận Jacoby 46 7.3 Thiết kế quỹ đạo 53 DANH MỤC HÌNH VẼ Hình 1.1 Robot MH3BM thực tế Hình 1.2 Góc nhìn từ phía trước Hình 1.3 Góc nhìn từ phía Hình 1.4 Góc nhìn từ cạnh ngang Hình 1.5 Thơng số kỹ thuật Robot MH3BM Hình 1.6 Bộ điều khiển FS100 cho robot MH3BM Hình 1.7 Thơng số kỹ thuật điều khiển FS100 Hình 2.1 Trường hợp chéo Hình 2.2 Trường hợp song song Hình 2.3 Trường hợp cắt Hình 2.4 Cách đặt vị trí tâm O0 Hình 2.5 Trường hợp khớp i khớp tịnh tiến Hình 2.6 Gán hệ trục tọa độ cho robot MH3BM 10 Hình 2.7 Giao diện ban đầu 13 Hình 2.8 Giao diện sau tính tốn 13 Hình 3.1 Giao diện ban đầu 17 Hình 3.2 Giao diện sau tính tốn ma trận Jacoby 18 Hình 5.1 Đồ thị quỹ đạo, vận tốc gia tốc khớp 24 Hình 5.2 Đồ thị quỹ đạo, vận tốc gia tốc khớp 25 Hình 5.3 Đồ thị quỹ đạo, vận tốc gia tốc khớp 26 Hình 5.4 Đồ thị quỹ đạo, vận tốc gia tốc khớp 27 Hình 5.5 Đồ thị quỹ đạo, vận tốc gia tốc khớp 28 Hình 5.6 Đồ thị quỹ đạo, vận tốc gia tốc khớp 29 Hình 6.1 Phần đế robot 30 Hình 6.2 Thanh nối 30 Hình 6.3 Thanh nối 31 Hình 6.4 Thanh nối 31 Hình 6.5 Thanh nối 32 Hình 6.6 Thanh nối 32 Hình 6.7 Khâu tác động cuối 33 Hình 6.8 Robot MH3BM hồn chỉnh 33 Hình 6.9 Thêm Toolbox vào solid work 34 Hình 6.10 Mơ hình simulink 34 Hình 6.11 Mơ hình điều khiển khơng gian khớp thuật tốn PID 35 Hình 6.12 Bộ PID cho khớp 35 Hình 6.13 Bộ PID cho khớp 36 Hình 6.14 Bộ PID cho khớp 36 Hình 6.15 Bộ PID cho khớp 37 Hình 6.16 Bộ PID cho khớp 37 Hình 6.17 Bộ PID cho khớp 37 Hình 6.18 Kết nối điều khiển vào mơ hình động lực học 38 Hình 6.19 Đáp ứng vị trí khớp 38 Hình 6.19 Đáp ứng vị trí khớp 39 Hình 6.19 Đáp ứng vị trí khớp 39 Hình 6.19 Đáp ứng vị trí khớp 39 Hình 6.19 Đáp ứng vị trí khớp 40 Hình 6.19 Đáp ứng vị trí khớp 40 DANH MỤC BẢNG BIỂU Bảng 2-1 Bảng D-H Robot MH3BM 10 Bảng 2-2 Các thông số từ Datasheet 11 Bảng 6-1 Thông số PID 35 ĐỀ TÀI Tìm hiểu tính tốn Robot MH3BM với yêu cầu: 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 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 Tính tốn ma trận Jacoby (thơng qua JH) viêt chương trình MatLab Tính tốn động học đảo vị trí Robot Thiết kế quỹ đạo chuyển động cho khớp Robot theo quỹ đạo dạng bậc Xây dựng mơ hình động lực học cho đối tượng ToolBox Simscape/MatLab CHƯƠNG GIỚI THIỆU ROBOT MH3BM 1.1 Lịch sử hình thành Robot Thuật ngữ “robot” lần xuất năm 1922 tác phẩm “Rossum’s Universal Robots” Karel Capek Theo tiếng Séc robot người làm tạp dịch Trong tác phẩm nhân vật Rossum trai ông tạo máy gần giống người để hầu hạ người Hơn 20 năm sau, ước mơ viễn tưởng Karel Capek bắt đầu thực Ngay sau chiến tranh giới thứ 2, Hoa Kỳ xuất tay máy chép hình điều khiển từ xa phịng thí nghiệm vật liệu phóng xạ Vào năm 1950 bên cạnh tay máy chép hình khí đó, xuất loại tay máy chép hình thủy lực điện từ, tay máy Minotaur I, tay máy Handyman General Electric Năm 1954 George C.Devol thiết kế thiết bị có tên “cơ cấu lề dùng để chuyển hàng hóa theo chương trình” Đến năm 1956 Devol với Joseph F.Engelber, kĩ sư trẻ công nghiệp hàng không, tạo loại robot công nghiệp năm 1959 công ty Unimation Chỉ đến năm 1975 cơng ty Unimation bắt đầu có lợi nhuận từ sản phẩm robot Chiếc robot công nghiệp đưa vào ứng dụng đầu tiên, năm 1961 nhà máy ô tô General Motors Trenton, New Jersey Năm 1967 Nhật Bản nhập robot từ công ty AMF Hoa Kỳ (American Machine and Foundry Company) Đến năm 1990 có 40 cơng ty Nhật Bản, có cơng ty khổng lồ Hitachi Mitsibishi, đưa thị trường quốc tế nhiều loại robot tiếng Từ năm 1980, vào năm 1990, áp dụng rộng rãi tiến kỹ thuật vi xử lý công nghệ thông tin, số lượng robot công nghiệp gia tăng, giá thành giảm rõ rệt, tính có nhiều bước tiến vượt bậc Nhờ robot cơng nghiệp có vị trí quan trọng dây truyền tự động sản xuất đại 1.2 Robot MH3BM Robot báo cáo robot MH3BM hãng Yaskawa tập đoàn hàng đầu giới sản xuất cung cấp sản phẩm lĩnh vực robot công nghiệp, biến tần, truyền động điện…Trong lĩnh vực robot công nghiệp, hãng sản xuất tất cảcác loại robot 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)… Robot MH3BM mơ hình robot tốc độ cao ứng dụng xử lý ngành dược phẩm, hóa chất y tế cho ứng dụng khác điều kiện môi trường khắc nghiệt Cánh tay robot cơng nghiệp MH3BM có khả tải trọng (3 kg), tốc độ chuyển động phạm vi làm việc tốt (cho phép lượt đầy đủ trục S chính) tốt lớp Cáp nguồn dẫn hướng qua đế thông qua ổ cắm Robot, cáp ứng dụng dẫn qua cánh tay robot, vừa giúp cài đặt dễ dàng vừa dễ lau chùi Lớp phủ bề mặt đặc biệt cho phép làm hydro peroxide Cấu trúc kèm theo niêm phong để cung cấp bảo vệ xâm nhập phù hợp với IP67 cho cổ tay chuẩn cho trục Nó tn thủ tiêu chuẩn phịng ISO loại 5, khơng cho phép tạo bụi bám dính Hình 1.1 Robot MH3BM ngồi thực tế 1.3 Ứng dụng công nghiệp Robot MH3BM ứng dụng cho dây chuyền sản xuất tự động, hoạt động mơi trường khắc nghiệt như: • Các trình sản xuất dược phẩm (pha chế), sản xuất thuốc, thử nghiệm lâm sàng • Mơi trường hóa chất • Mơi trường lắp rấp phịng 1.4 Thông số kĩ thuật - Số bậc tự do: - Kiểu khớp: khớp quay - Vùng không gian làm việc kết cấu khí: Hình 1.2 Góc nhìn từ phía trước Hình 1.3 Góc nhìn từ phía Hình 1.4 Góc nhìn từ cạnh ngang CHƯƠNG PHỤ LỤC 7.1 Giao diện động học thuận function varargout = donghocthuan(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, 'gui_Singleton', gui_Singleton, 'gui_OpeningFcn', @donghocthuan_OpeningFcn, 'gui_OutputFcn', @donghocthuan_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 donghocthuan is made visible function donghocthuan_OpeningFcn(hObject, eventdata, handles, varargin) % This function has no output args, see OutputFcn % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % varargin command line arguments to donghocthuan (see VARARGIN) % Choose default command line output for donghocthuan handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes donghocthuan wait for user response (see UIRESUME) % uiwait(handles.figure1); % - Outputs from this function are returned to the command line function varargout = donghocthuan_OutputFcn(hObject, eventdata, handles) % varargout cell array for returning output args (see VARARGOUT); % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Get default command line output from handles structure varargout{1} = handles.output; function nhap_theta1_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) 41 % Hints: get(hObject,'String') returns contents of nhap_theta1 as text % str2double(get(hObject,'String')) returns contents of nhap_theta1 as a double theta1 = str2num(get(handles.nhap_theta1,'string')); if (theta1 > 180) theta1 = 180; end if (theta1 < -180) theta1 = -180; end set(handles.nhap_theta1,'string',num2str(theta1)); % - Executes during object creation, after setting all properties function nhap_theta1_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta6_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta6 (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 nhap_theta6 as text % str2double(get(hObject,'String')) returns contents of nhap_theta6 as a double theta6 = str2num(get(handles.nhap_theta6,'string')); if (theta6 > 360) theta6 = 360; end if (theta6 < -360) theta6 = -360; end set(handles.nhap_theta6,'string',num2str(theta6)); % - Executes during object creation, after setting all properties function nhap_theta6_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta6 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta5_Callback(hObject, eventdata, handles) 42 % hObject % eventdata % handles handle to nhap_theta5 (see GCBO) reserved - to be defined in a future version of MATLAB structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of nhap_theta5 as text % str2double(get(hObject,'String')) returns contents of nhap_theta5 as a double theta5 = str2num(get(handles.nhap_theta5,'string')); if (theta5 > 120) theta5 = 120; end if (theta5 < -120) theta5 = -120; end set(handles.nhap_theta5,'string',num2str(theta5)); % - Executes during object creation, after setting all properties function nhap_theta5_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta5 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta4_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta4 (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 nhap_theta4 as text % str2double(get(hObject,'String')) returns contents of nhap_theta4 as a double theta4 = str2num(get(handles.nhap_theta4,'string')); if (theta4 > 170) theta4 = 170; end if (theta4 < -170) theta4 = -170; end set(handles.nhap_theta4,'string',num2str(theta4)); % - Executes during object creation, after setting all properties function nhap_theta4_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta4 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end 43 function nhap_theta2_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta2 (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 nhap_theta2 as text % str2double(get(hObject,'String')) returns contents of nhap_theta2 as a double theta2 = str2num(get(handles.nhap_theta2,'string')); if (theta2 > 90) theta2 = 90; end if (theta2 < -85) theta2 = -85; end set(handles.nhap_theta2,'string',num2str(theta2)); % - Executes during object creation, after setting all properties function nhap_theta2_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta2 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta3_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta3 (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 nhap_theta3 as text % str2double(get(hObject,'String')) returns contents of nhap_theta3 as a double theta3 = str2num(get(handles.nhap_theta3,'string')); if (theta3 > 260) theta3 = 260; end if (theta3 < -105) theta3 = -105; end set(handles.nhap_theta3,'string',num2str(theta3)); % - Executes during object creation, after setting all properties function nhap_theta3_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called 44 % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end % - Executes on button press in Tinh function Tinh_Callback(hObject, eventdata, handles) % hObject handle to Tinh (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) theta1 = str2num(get(handles.nhap_theta1,'string')); theta2 = str2num(get(handles.nhap_theta2,'string')); theta3 = str2num(get(handles.nhap_theta3,'string')); theta4 = str2num(get(handles.nhap_theta4,'string')); theta5 = str2num(get(handles.nhap_theta5,'string')); theta6 = str2num(get(handles.nhap_theta6,'string')); %%Tinh ma tran T06 a2 = 290; d4 = 270; A1 = [cosd(theta1) sind(theta1) 0;sind(theta1) -cosd(theta1) 0;0 0;0 0 1]; A2 = [cosd(theta2) -sind(theta2) a2*cosd(theta2);sind(theta2) cosd(theta2) a2*sind(theta2);0 0;0 0 1]; A3 = [cosd(theta3) sind(theta3) 0;sind(theta3) -cosd(theta3) 0;0 0;0 0 1]; A4 = [cosd(theta4) -sind(theta4) 0;sind(theta4) cosd(theta4) 0;0 -1 d4;0 0 1]; A5 = [cosd(theta5) sind(theta5) 0;sind(theta5) -cosd(theta5) 0;0 0;0 0 1]; A6 = [cosd(theta6) -sind(theta6) 0;sind(theta6) cosd(theta6) 0;0 0;0 0 1]; T = A1*A2*A3*A4*A5*A6; set(handles.T11,'string',num2str(T(1,1))); set(handles.T12,'string',num2str(T(1,2))); set(handles.T13,'string',num2str(T(1,3))); set(handles.T14,'string',num2str(T(1,4))); set(handles.T21,'string',num2str(T(2,1))); set(handles.T22,'string',num2str(T(2,2))); set(handles.T23,'string',num2str(T(2,3))); set(handles.T24,'string',num2str(T(2,4))); set(handles.T31,'string',num2str(T(3,1))); set(handles.T32,'string',num2str(T(3,2))); set(handles.T33,'string',num2str(T(3,3))); set(handles.T34,'string',num2str(T(3,4))); set(handles.T41,'string',num2str(T(4,1))); set(handles.T42,'string',num2str(T(4,2))); set(handles.T43,'string',num2str(T(4,3))); set(handles.T44,'string',num2str(T(4,4))); % - Executes on button press in Xoa function Xoa_Callback(hObject, eventdata, handles) % hObject handle to Xoa (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) set(handles.nhap_theta1,'string',' '); set(handles.nhap_theta2,'string',' '); set(handles.nhap_theta3,'string',' '); set(handles.nhap_theta4,'string',' '); set(handles.nhap_theta5,'string',' '); set(handles.nhap_theta6,'string',' '); 45 set(handles.T11,'string',' set(handles.T12,'string',' set(handles.T13,'string',' set(handles.T14,'string',' set(handles.T21,'string',' set(handles.T22,'string',' set(handles.T23,'string',' set(handles.T24,'string',' set(handles.T31,'string',' set(handles.T32,'string',' set(handles.T33,'string',' set(handles.T34,'string',' set(handles.T41,'string',' set(handles.T42,'string',' set(handles.T43,'string',' set(handles.T44,'string',' '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); 7.2 Giao diện tính ma trận Jacoby function varargout = Jacoby(varargin) % JACOBY MATLAB code for Jacoby.fig % JACOBY, by itself, creates a new JACOBY or raises the existing % singleton* % % H = JACOBY returns the handle to a new JACOBY or the handle to % the existing singleton* % % JACOBY('CALLBACK',hObject,eventData,handles, ) calls the local % function named CALLBACK in JACOBY.M with the given input arguments % % JACOBY('Property','Value', ) creates a new JACOBY or raises the % existing singleton* Starting from the left, property value pairs are % applied to the GUI before Jacoby_OpeningFcn gets called An % unrecognized property name or invalid value makes property application % stop All inputs are passed to Jacoby_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 Jacoby % Last Modified by GUIDE v2.5 06-Jan-2022 22:36:35 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, 'gui_Singleton', gui_Singleton, 'gui_OpeningFcn', @Jacoby_OpeningFcn, 'gui_OutputFcn', @Jacoby_OutputFcn, 'gui_LayoutFcn', [] , 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end 46 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 Jacoby is made visible function Jacoby_OpeningFcn(hObject, eventdata, handles, varargin) % This function has no output args, see OutputFcn % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % varargin command line arguments to Jacoby (see VARARGIN) % Choose default command line output for Jacoby handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes Jacoby wait for user response (see UIRESUME) % uiwait(handles.figure1); % - Outputs from this function are returned to the command line function varargout = Jacoby_OutputFcn(hObject, eventdata, handles) % varargout cell array for returning output args (see VARARGOUT); % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Get default command line output from handles structure varargout{1} = handles.output; function nhap_theta1_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta1 (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 nhap_theta1 as text % str2double(get(hObject,'String')) returns contents of nhap_theta1 as a double theta1 = str2num(get(handles.nhap_theta1,'string')); if (theta1 > 180) theta1 = 180; end if (theta1 < -180) theta1 = -180; end set(handles.nhap_theta1,'string',num2str(theta1)); % - Executes during object creation, after setting all properties function nhap_theta1_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called 47 % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta6_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta6 (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 nhap_theta6 as text % str2double(get(hObject,'String')) returns contents of nhap_theta6 as a double theta6 = str2num(get(handles.nhap_theta6,'string')); if (theta6 > 360) theta6 = 360; end if (theta6 < -360) theta6 = -360; end set(handles.nhap_theta6,'string',num2str(theta6)); % - Executes during object creation, after setting all properties function nhap_theta6_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta6 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta5_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta5 (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 nhap_theta5 as text % str2double(get(hObject,'String')) returns contents of nhap_theta5 as a double theta5 = str2num(get(handles.nhap_theta5,'string')); if (theta5 > 120) theta5 = 120; end if (theta5 < -120) theta5 = -120; end set(handles.nhap_theta5,'string',num2str(theta5)); % - Executes during object creation, after setting all properties 48 function nhap_theta5_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta5 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta4_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta4 (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 nhap_theta4 as text % str2double(get(hObject,'String')) returns contents of nhap_theta4 as a double theta4 = str2num(get(handles.nhap_theta4,'string')); if (theta4 > 170) theta4 = 170; end if (theta4 < -170) theta4 = -170; end set(handles.nhap_theta4,'string',num2str(theta4)); % - Executes during object creation, after setting all properties function nhap_theta4_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta4 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta2_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta2 (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 nhap_theta2 as text % str2double(get(hObject,'String')) returns contents of nhap_theta2 as a double theta2 = str2num(get(handles.nhap_theta2,'string')); if (theta2 > 90) theta2 = 90; end 49 if (theta2 < -85) theta2 = -85; end set(handles.nhap_theta2,'string',num2str(theta2)); % - Executes during object creation, after setting all properties function nhap_theta2_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta2 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function nhap_theta3_Callback(hObject, eventdata, handles) % hObject handle to nhap_theta3 (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 nhap_theta3 as text % str2double(get(hObject,'String')) returns contents of nhap_theta3 as a double theta3 = str2num(get(handles.nhap_theta3,'string')); if (theta3 > 260) theta3 = 260; end if (theta3 < -105) theta3 = -105; end set(handles.nhap_theta3,'string',num2str(theta3)); % - Executes during object creation, after setting all properties function nhap_theta3_CreateFcn(hObject, eventdata, handles) % hObject handle to nhap_theta3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end % - Executes on button press in Tinh function Tinh_Callback(hObject, eventdata, handles) % hObject handle to Tinh (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) theta1 = str2num(get(handles.nhap_theta1,'string')); theta2 = str2num(get(handles.nhap_theta2,'string')); theta3 = str2num(get(handles.nhap_theta3,'string')); theta4 = str2num(get(handles.nhap_theta4,'string')); 50 theta5 = str2num(get(handles.nhap_theta5,'string')); theta6 = str2num(get(handles.nhap_theta6,'string')); %%Tinh ma tran T06 a2 = 290; d4 = 270; A1 = [cosd(theta1) sind(theta1) 0;sind(theta1) -cosd(theta1) 0;0 0;0 0 1]; A2 = [cosd(theta2) -sind(theta2) a2*cosd(theta2);sind(theta2) cosd(theta2) a2*sind(theta2);0 0;0 0 1]; A3 = [cosd(theta3) sind(theta3) 0;sind(theta3) -cosd(theta3) 0;0 0;0 0 1]; A4 = [cosd(theta4) -sind(theta4) 0;sind(theta4) cosd(theta4) 0;0 -1 d4;0 0 1]; A5 = [cosd(theta5) sind(theta5) 0;sind(theta5) -cosd(theta5) 0;0 0;0 0 1]; A6 = [cosd(theta6) -sind(theta6) 0;sind(theta6) cosd(theta6) 0;0 0;0 0 1]; % Tính Ti6 T56=A6; T46=A5*A6; T36=A4*A5*A6; T26=A6*A5*A3*A4; T16=A6*A2*A3*A4*A5; T06=A1*A2*A3*A4*A5*A6; %Tinh ma tran JH JH11 = T06(2,1)*T06(1,4)-T06(1,1)*T06(2,4); %c?t JH21 = T16(2,1)*T16(1,4)-T16(1,1)*T16(2,4); JH31 = T26(2,1)*T26(1,4)-T26(1,1)*T26(2,4); JH41 = T36(2,1)*T36(1,4)-T36(1,1)*T36(2,4); JH51 = T46(2,1)*T46(1,4)-T46(1,1)*T46(2,4); JH61 = T56(2,1)*T56(1,4)-T56(1,1)*T56(2,4); JH12 JH22 JH32 JH42 JH52 JH62 = = = = = = T06(2,2)*T06(1,4)-T06(1,2)*T06(2,4); T16(2,2)*T16(1,4)-T16(1,2)*T16(2,4); T26(2,2)*T26(1,4)-T26(1,2)*T26(2,4); T36(2,2)*T36(1,4)-T36(1,2)*T36(2,4); T46(2,2)*T46(1,4)-T46(1,2)*T46(2,4); T56(2,2)*T56(1,4)-T56(1,2)*T56(2,4); %c?t JH13 JH23 JH33 JH43 JH53 JH63 = = = = = = T06(2,3)*T06(1,4)-T06(1,3)*T06(2,4); %c?t T16(2,3)*T16(1,4)-T16(1,3)*T16(2,4); T26(2,3)*T26(1,4)-T26(1,3)*T26(2,4); T36(2,3)*T36(1,4)-T36(1,3)*T36(2,4); T46(2,3)*T46(1,4)-T46(1,3)*T46(2,4); T56(2,3)*T56(1,4)-T56(1,3)*T56(2,4); JH14 JH24 JH34 JH44 JH54 JH64 = = = = = = T06(3,1); T16(3,1); T26(3,1); T36(3,1); T46(3,1); T56(3,1); %c?t JH15 JH25 JH35 JH45 JH55 JH65 = = = = = = T06(3,2); T16(3,2); T26(3,2); T36(3,2); T46(3,2); T56(3,2); %c?t JH16 JH26 JH36 JH46 = = = = T06(3,3); T16(3,3); T26(3,3); T36(3,3); %c?t 51 JH56 = T46(3,3); JH66 = T56(3,3); Jh = [JH11,JH21,JH31,JH41,JH51,JH61; JH12,JH22,JH32,JH42,JH52,JH62; JH13,JH23,JH33,JH43,JH53,JH63; JH14,JH24,JH34,JH44,JH54,JH64; JH15,JH25,JH35,JH45,JH55,JH65; JH16,JH26,JH36,JH46,JH56,JH66]; R0n = T06(1:3,1:3); %ma tran R0n %tinh ma tran Jacoby J = [R0n,zeros(3,3);zeros(3,3),R0n]*Jh; %ghi data set(handles.J11,'string',num2str(J(1,1))); set(handles.J12,'string',num2str(J(1,2))); set(handles.J13,'string',num2str(J(1,3))); set(handles.J14,'string',num2str(J(1,4))); set(handles.J15,'string',num2str(J(1,5))); set(handles.J16,'string',num2str(J(1,6))); set(handles.J21,'string',num2str(J(2,1))); set(handles.J22,'string',num2str(J(2,2))); set(handles.J23,'string',num2str(J(2,3))); set(handles.J24,'string',num2str(J(2,4))); set(handles.J25,'string',num2str(J(2,5))); set(handles.J26,'string',num2str(J(2,6))); set(handles.J31,'string',num2str(J(3,1))); set(handles.J32,'string',num2str(J(3,2))); set(handles.J33,'string',num2str(J(3,3))); set(handles.J34,'string',num2str(J(3,4))); set(handles.J35,'string',num2str(J(3,5))); set(handles.J36,'string',num2str(J(3,6))); set(handles.J41,'string',num2str(J(4,1))); set(handles.J42,'string',num2str(J(4,2))); set(handles.J43,'string',num2str(J(4,3))); set(handles.J44,'string',num2str(J(4,4))); set(handles.J45,'string',num2str(J(4,5))); set(handles.J46,'string',num2str(J(4,6))); set(handles.J51,'string',num2str(J(5,1))); set(handles.J52,'string',num2str(J(5,2))); set(handles.J53,'string',num2str(J(5,3))); set(handles.J54,'string',num2str(J(5,4))); set(handles.J55,'string',num2str(J(5,5))); set(handles.J56,'string',num2str(J(5,6))); set(handles.J61,'string',num2str(J(6,1))); set(handles.J62,'string',num2str(J(6,2))); set(handles.J63,'string',num2str(J(6,3))); set(handles.J64,'string',num2str(J(6,4))); set(handles.J65,'string',num2str(J(6,5))); set(handles.J66,'string',num2str(J(6,6))); % - Executes on button press in Xoa function Xoa_Callback(hObject, eventdata, handles) % hObject handle to Xoa (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) set(handles.nhap_theta1,'string',' '); set(handles.nhap_theta2,'string',' '); set(handles.nhap_theta3,'string',' '); set(handles.nhap_theta4,'string',' '); set(handles.nhap_theta5,'string',' '); set(handles.nhap_theta6,'string',' '); set(handles.J11,'string',' '); 52 set(handles.J12,'string',' set(handles.J13,'string',' set(handles.J14,'string',' set(handles.J15,'string',' set(handles.J16,'string',' set(handles.J21,'string',' set(handles.J22,'string',' set(handles.J23,'string',' set(handles.J24,'string',' set(handles.J25,'string',' set(handles.J26,'string',' set(handles.J31,'string',' set(handles.J32,'string',' set(handles.J33,'string',' set(handles.J34,'string',' set(handles.J35,'string',' set(handles.J36,'string',' set(handles.J41,'string',' set(handles.J42,'string',' set(handles.J43,'string',' set(handles.J44,'string',' set(handles.J45,'string',' set(handles.J46,'string',' set(handles.J51,'string',' set(handles.J52,'string',' set(handles.J53,'string',' set(handles.J54,'string',' set(handles.J55,'string',' set(handles.J56,'string',' set(handles.J61,'string',' set(handles.J62,'string',' set(handles.J63,'string',' set(handles.J64,'string',' set(handles.J65,'string',' set(handles.J66,'string',' '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); '); 7.3 Thiết kế quỹ đạo t = [0:0.005:2]; tAB = 2; %2s %diem A[100,20,-40,0,35,300]?B[130,-20,70,-40,100,-200] t1A = 100; t1B = 130; t2A = 20; t2B = -20; t3A = -40; t3B = 70; t4A = 0; t4B = -40; t5A = 35; t5B = 100; t6A = 300; t6B = -200; % khop figure(1) q1_1 = t1A + 3*(t1B - t1A)/(tAB^2).*(t.^2) - 2*(t1Bt1A)/(tAB^3).*(t.^3); q1_2 = 2*(3*(t1B - t1A)/(tAB^2)).*t - 3*2*(t1B t1A)/(tAB^3).*(t.^2); q1_3 = 2*(3*(t1B - t1A)/(tAB^2)) - 2*3*2*(t1B - t1A)/(tAB^3).*t; subplot(3,1,1) plot(t,q1_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q1_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q1_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop 53 figure(2) q2_1 = t2A + 3*(t2B - t2A)/(tAB^2).*(t.^2) - 2*(t2B t2A)/(tAB^3).*(t.^3); q2_2 = 2*(3*(t2B - t2A)/(tAB^2)).*t - 3*2*(t2B - t2A)/(tAB^3).*(t.^2); q2_3 = 2*(3*(t2B - t2A)/(tAB^2)) - 2*3*2*(t2B - t2A)/(tAB^3).*t; subplot(3,1,1) plot(t,q2_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q2_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q2_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop figure(3) q3_1 = t3A + 3*(t3B - t3A)/(tAB^2).* (t.^2) - 2*(t3B t3A)/(tAB^3).*(t.^3); q3_2 = 2*(3*(t3B - t3A)/(tAB^2)).*t - 3*2*(t3B - t3A)/(tAB^3).*(t.^2); q3_3 = 2*(3*(t3B - t3A)/(tAB^2)) - 2*3*2*(t3B - t3A)/(tAB^3).*t; subplot(3,1,1) plot(t,q3_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q3_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q3_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop figure(4) q4_1 = t4A + 3*(t4B - t4A)/(tAB^2).*(t.^2) - 2*(t4B t4A)/(tAB^3).*(t.^3); q4_2 = 2*(3*(t4B - t4A)/(tAB^2)).*t - 3*2*(t4B t4A)/(tAB^3).*(t.^2); q4_3 = 2*(3*(t4B - t4A)/(tAB^2)) - 2*3*2*(t4B - t4A)/(tAB^3).*t; subplot(3,1,1) plot(t,q4_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q4_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q4_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop figure(5) q5_1 = t5A + 3*(t5B - t5A)/(tAB^2).*(t.^2) - 2*(t5B t5A)/(tAB^3).*(t.^3); q5_2 = 2*(3*(t5B - t5A)/(tAB^2)).*t - 3*2*(t5B t5A)/(tAB^3).*(t.^2); q5_3 = 2*(3*(t5B - t5A)/(tAB^2)) - 2*3*2*(t5B - t5A)/(tAB^3).*t; subplot(3,1,1) plot(t,q5_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q5_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q5_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop figure(6) q6_1 = t6A + 3*(t6B - t6A)/(tAB^2).*(t.^2) - 2*(t6B t6A)/(tAB^3).*(t.^3); q6_2 = 2*(3*(t6B - t6A)/(tAB^2)).*t - 3*2*(t6B t6A)/(tAB^3).*(t.^2); q6_3 = 2*(3*(t6B - t6A)/(tAB^2)) - 2*3*2*(t6B - t6A)/(tAB^3).*t; subplot(3,1,1) plot(t,q6_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q6_2,'LineWidth',2);grid on;legend('V?n t?c'); 54 subplot(3,1,3) plot(t,q6_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); 55 ... học cho robot Matlab nhóm thực việc thiết kế Solid work sau chuyển phần mềm Matlab 6.1 Thiết kế Solid Work Sử dụng phần mềm Solid Work 2020 ta thiết kế robot MH3BM ta thiết kế phần Robot MH3BM. .. 1]; 1]; 1]; 21 CHƯƠNG THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 5.1 Lý thuyết Thiết kế quỹ đạo chuyển động cho robot có liên quan trực tiếp đến toán điều khiển robot Các quỹ đạo thiết kế phải đảm bảo mặt logic,... phía Hình 1.4 Góc nhìn từ cạnh ngang Hình 1.5 Thơng số kỹ thuật Robot MH3BM Hình 1.6 Bộ điều khiển FS100 cho robot MH3BM Hình 1.7 Thơng số kỹ thuật điều khiển FS100 1.5 Hình ảnh video làm việc thực