Thiết kế và mô phỏng điều khiển robot 3 bậc tự do RRR

Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ ĐỒ ÁN MƠN HỌC: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN Mã HP: ME4336 TỬ Họ tên sinh viên: Phạm Duy Trọng MSSV: 20134134 – KT CĐT 02 Giảng viên hướng dẫn: TS Nguyễn Danh Trường Bộ môn: Cơ học vật liệu kết cấu NHIỆM VỤ ĐỒ ÁN MÔN HỌC Đề Tài: Thiết kế mô điều khiển robot bậc tự RRR Cài công cụ cần thiết: Phần mềm Matlab, Solidworks Các tài kiệu tham khảo - Các tài liệu tham khảo phần mềm Matlab, công cụ Simulink, công cụ Simmechanics - Các tài liệu lí thuyết điều khiển - Sách Robot công nghiệp, Động lực học hệ nhiều vật, … Nội dung phần thuyết minh tính tốn - Vẽ thiết kế Robot Solidworks - Tính tốn động học thuận ngược - Thiết kế điều khiển mô Matlab - Kết đồ án Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường MỤC LỤC Chương I: Thiết kế robot 3DOF – RRR phần mềm Solidwork Thiết kế đặt hệ trục tọa độ cho khâu 1.1 Thiết kế giá đỡ (khâu 0) 1.2 Thiết kế khâu 15 1.3 Thiết kế khâu 16 1.4 Thiết kế khâu 16 Lắp ráp khâu 17 Chương II: Tính tốn động học động lực học cho robot 22 Thiết lập bảng thông số D-H cho robot 3DOF 22 Giải toán động học thuận 24 2.1 Ma trận D-H 24 2.2 Vận tốc góc khâu 26 2.3 Gia tốc góc khâu 30 Giải toán động học ngược 32 CHƯƠNG III: Thiết kế mạch mô điều khiển GUI 39 Thiết kế mô điều khiển GUI với version First Generation 39 1.1 Giới thiệu khối robot 41 1.2 Thêm khối điều khiển 42 Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot 1.3 GVHD: TS Nguyễn Danh Trường Kết chạy mô sau thêm khối điều khiển 44 1.4 Lập trình giao diện GUI matlab 45 1.4.1 Thiết kế giao diện GUI 45 1.4.2 Lập trình giao diện file (.m) 48 Thiết kế mô điều khiển GUI với version Second Generation 53 2.1 Giới thiệu khối robot 53 2.2 Thêm khối điều khiển: PID, Step, S-PS, PS-S 54 2.3 Lập trình giao diện GUI matlab 57 2.3.1 Thiết kế giao diện GUI 57 2.3.2 Lập trình GUI file (.m) 63 Kết luận: 69 Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường Chương I: Thiết kế robot 3DOF – RRR phần mềm Solidwork Khớp quay Khớp quay Khâu Khâu Khâu Khớp quay Giá đỡ Thiết kế đặt hệ trục tọa độ cho khâu Do trình tự thiết kế đặt hệ trục tọa độ khâu tương tự lên sau em xin trình bày rõ ràng thiết kế khâu, khâu khác em xin phép lấy hình 1.1 Thiết kế giá đỡ (khâu 0) Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường • Trên mặt phẳng Top vẽ sketch với biên dạng đường tròn với D=300mm Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường • Sử dụng lệnh extrude • Sử dụng lệnh chamfer: Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường • Trên mặt phẳng đáy vẽ sketch có biên dạng sau: Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường • Sử dụng lệnh extrude: • Dùng lệnh chamfer Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường • Vẽ sketch mặt phẳng sau: Mặt phẳng • Sau extrude: • Tương tự vẽ sketch mặt phẳng sau extrude: Sinh viên thực hiện: Phạm Duy Trọng Page | Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường • Sử dụng lệnh mirror qua mặt phẳng front: Sinh viên thực hiện: Phạm Duy Trọng Page | 10 Thiết kế mô điều khiển robot - GVHD: TS Nguyễn Danh Trường PS-S: chuyển đổi đầu vào tín hiệu vật lý thành tín hiệu cho khối Simulink Sinh viên thực hiện: Phạm Duy Trọng Page | 56 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường - S-PS: chuyển đổi tín hiệu từ khối Simulink thành tín hiệu vật lý - Gain: khối thu từ tín hiệu đầu vào chuyển đổi sang đầu - Scope: khối thu thập liệu theo khối thực hay hoạt động 2.3 Lập trình giao diện GUI matlab 2.3.1 Thiết kế giao diện GUI Trong sổ comment windowns gõ lệnh “guide” ta giao diện sau: Sinh viên thực hiện: Phạm Duy Trọng Page | 57 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường Chọn Blank GUI bắt đầu thiết kế: - Sử dụng công cụ thiết kế như: ✓ Push Button: nút bấm ✓ Edit Text: nơi kí tự nhập vào từ người dùng, người dùng dùng thay đổi ✓ Slider: trượt có trượt chạy ✓ Static Text: kí tự thị thông qua Callback, thông thường để viết nhãn cho biểu tượng, người dùng thay đổi nội dung Sinh viên thực hiện: Phạm Duy Trọng Page | 58 Thiết kế mô điều khiển robot - GVHD: TS Nguyễn Danh Trường Ví dụ khâu 1: ✓ Nút tăng: Sinh viên thực hiện: Phạm Duy Trọng Page | 59 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường ✓ Nút giảm: Sinh viên thực hiện: Phạm Duy Trọng Page | 60 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường ✓ Edit Text1: - Các nút START, STOP, BT Nguoc tương tự nút tăng hay giảm khác chỗ: khai báo biến (hay tên thẻ Tag) không giống - Px, Py, Pz tọa độ điểm cuối khâu thao tác dùng để giải tốn ngược, ví dụ thiết kế Px ✓ Slider1: Sinh viên thực hiện: Phạm Duy Trọng Page | 61 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường ✓ Edit Text: Sinh viên thực hiện: Phạm Duy Trọng Page | 62 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường 2.3.2 Lập trình GUI file (.m) Sau ta thiết kế xong nhấn Run lưu file (.gui) vào thư mục Robot ta có thêm file m để lập trình thêm Code để điều khiển robot - Thêm code vào btnTangkhau1 function btnTangkhau1_Callback(hObject, eventdata, handles) % hObject handle to btnTangkhau1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) handles = guidata(hObject); ModelName = 'Robot3Dof'; handles.Goc1 = handles.Goc1 + 10; %Moi lan tang 10 deg va gan bang bien handles.Goc1 set(handles.edit1,'string',num2str(handles.Goc1)); Sinh viên thực hiện: Phạm Duy Trọng Page | 63 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường handles.Goc2 = handles.Goc2; %gan bang bien handles.Goc2=0 deg set(handles.edit2,'string',num2str(handles.Goc2)); handles.Goc3 = handles.Goc3; %gan bang bien handles.Goc3=0 deg set(handles.edit3,'string',num2str(handles.Goc3)); set_param([ModelName '/Actuator1'],'after',num2str(handles.Goc1)); set_param([ModelName '/Actuator2'],'after',num2str(handles.Goc2)); set_param([ModelName '/Actuator3'],'after',num2str(handles.Goc3)); b1=[cosd(handles.Goc1) sind(handles.Goc1) ;sind(handles.Goc1) -cosd(handles.Goc1) ;0 900;0 0 1];%ma tran DH khâu b2=[cosd(handles.Goc2) -sind(handles.Goc2) 650*cosd(handles.Goc2) ;sind(handles.Goc2) cosd(handles.Goc2) 650*sind(handles.Goc2) ;0 ;0 0 1];%ma tran DH khâu b3=[cosd(handles.Goc3) -sind(handles.Goc3) 400*cosd(handles.Goc3);sind(handles.Goc3) cosd(handles.Goc3) 400*sind(handles.Goc3); 0 ; 0 1];%ma tran DH khâu c=b1*b2*b3;%phuong trinh dongng hoc xac dinh khâu thao tác px=c(1,4);%lay gia tri hang 1, cot tu ma tran c py=c(2,4);%lay gia tri hang 2, cot tu ma tran c pz=c(3,4);%lay gia tri hang 3, cot tu ma tran c set(handles.edit4,'string',num2str(px));%chuyen chuoi gia tri px dang ki tu va dat vao edti4 set(handles.edit5,'string',num2str(py));%chuyen chuoi gia tri px dang ki tu va dat vao edti5 set(handles.edit6,'string',num2str(pz));%chuyen chuoi gia tri px dang ki tu va dat vao edti6 guidata(hObject,handles); btnTangkhau2, btnTangkhau3 khác với btnTangkhau1 chỗ: ✓ btnTangkhau2 handles.Goc1 = handles.Goc1; set(handles.edit1,'string',num2str(handles.Goc1)); handles.Goc2 = handles.Goc2 + 10; %Moi lan tang 10 deg, va gan bang bien handles.Goc2 set(handles.edit2,'string',num2str(handles.Goc2)); handles.Goc3 = handles.Goc3; - Sinh viên thực hiện: Phạm Duy Trọng Page | 64 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường set(handles.edit3,'string',num2str(handles.Goc3)); ✓ btnTangkhau3 handles.Goc1 = handles.Goc1; set(handles.edit1,'string',num2str(handles.Goc1)); handles.Goc2 = handles.Goc2; set(handles.edit2,'string',num2str(handles.Goc2)); handles.Goc3 = handles.Goc3 + 10; %Moi lan tang 10 deg va gan bang bien handles.Goc3 set(handles.edit3,'string',num2str(handles.Goc3)); - Thêm code vào btnGiamkhau1 (tương tự với btnGiamkhau2, btnGiamkhau3): giống btnTangkhau1, btnTangkhau2, btnTangkhau3 khác chỗ thay dấu ‘+’ dấu ‘-’, thay tăng 10deg giảm 10deg - btnStart: thêm code function btnStart_Callback(hObject, eventdata, handles) handles = guidata(hObject); global Tenrobot Tenrobot='Robot3Dof'; handles.modelname=Tenrobot; % Bien ten model can dieu khien handles.Goc1 = 0; %Bien goc khau handles.Fbgoc1 = 0; %Feedback goc khau handles.Goc2 = 0; %Bien goc khau handles.Fbgoc2 = 0; %Feedback goc khau handles.Goc3 = 0; %Bien goc khau handles.Fbgoc3 = 0; %Feedback goc khau % Kiem tra load simulink model if ~modelIsLoaded(handles.modelname) load_system(handles.modelname); end set_param(handles.modelname,'BlockReduction','off'); set_param(handles.modelname,'StopTime','inf'); set_param(handles.modelname,'SimulationMode','normal'); % model start thi call funtion: localAddEventListener set_param(handles.modelname,'StartFcn','localAddEventListe ner'); % cho run simulink set_param(handles.modelname,'SimulationCommand','start'); % an nut start set(hObject,'Enable','off'); % hien nut stop set(handles.btnStop,'Enable','on'); % save su thay doi bien toi GUI Sinh viên thực hiện: Phạm Duy Trọng Page | 65 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường guidata(hObject,handles); - btnStop: thêm Code function btnStop_Callback(hObject, eventdata, handles) set_param(handles.modelname,'SimulationCommand','stop'); set_param(handles.modelname,'StopTime','10'); set_param(handles.modelname,'StartFcn',''); set(hObject,'Enable','off'); set(handles.btnStart,'Enable','on'); set_param([handles.modelname '/Actuator1'],'after','0'); set_param([handles.modelname '/Actuator2'],'after','0'); set_param([handles.modelname '/Actuator3'],'after','0'); guidata(hObject,handles); try catch ME if exist('hf','var')&&~isempty(hf)&&~ishandle(hf) delete(hf) end bdclose(all) end - Thêm hàm sau: function modelLoaded=modelIsLoaded(modelName) try modelLoaded=~isempty(find_system('Type','block_diagram','N ame',modelName)); catch ME modelLoaded=False; end %%%%%%%%%%%%%%%%%%%%%%%%%%% function eventhandle=localAddEventListener eventhandle=add_exec_event_listener('Robot3Dof/sensor','Po stOutputs',@localEventListener); % Ham load su kien moi co bien thay doi function localEventListener(block, eventdata) simData=block.OutputPort(1).Data; %lay bien dung tengui.fig de khong bi loi handles=guidata(BangDieuKhien); handles.fbGoc1=int16(simData(1)); handles.fbGoc2=int16(simData(2)); handles.fbGoc3=int16(simData(3)); set(handles.edit1,'String',num2str(handles.fbGoc1)); set(handles.edit2,'String',num2str(handles.fbGoc2)); set(handles.edit3,'String',num2str(handles.fbGoc3)); drawnow; - btnBTN: giải toán ngược Sinh viên thực hiện: Phạm Duy Trọng Page | 66 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường ModelName = 'Robot3Dof'; global var; %khai bao bien px=get(handles.slider1,'value');%lay gia tri tu slider4 gan vào bien px set(handles.edit4,'string',num2str(px));%chuyen chuoi giá tri px thành ki tu va dat no vao edit4 py=get(handles.slider2,'value'); set(handles.edit5,'string',num2str(py)); pz=get(handles.slider3,'value'); set(handles.edit6,'string',num2str(pz)); %%%%%%%tim cac goc theta d1=900; a2=650; a3=400; theta1=atan2d(py,px); m=cosd(theta1)*px+sind(theta1)*py; n=pz-d1; theta3=acosd((m^2+n^2-a3^2-a2^2)/(2*a2*a3)); theta2=atan2d(((cosd(theta3)*a3+a2)*nm*sind(theta3)*a3),(cosd(theta3)*a3+a2)*m+n*sind(theta3)*a 3); guidata(hObject,handles);%luu su thay doi set(handles.edit1,'string',num2str(theta1));%voi giá tri theta1 tim duoc chuyen dang ki tu dat gia tri vao edit1 set_param([ModelName '/Actuator1'],'after',num2str(theta1))%dat gia tri theta1 vào khoi silder gain mach dk set(handles.edit2,'string',num2str(theta2)); set_param([ModelName '/Actuator2'],'after',num2str(theta2)) set(handles.edit3,'string',num2str(theta3)); set_param([ModelName '/Actuator3'],'after',num2str(theta3)) - slider1: thêm code px=get(handles.slider1,'value');%lay gia tri tu slider4 gan vao bien px set(handles.edit4,'string',num2str(px));%dat px vao edit4 - slide2, slider3 tương tự slider1 Sinh viên thực hiện: Phạm Duy Trọng Page | 67 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường ❖ Kết thu sau điều khiển robot giao diện điều khiển: - Bài toán thuận - Bài toán nghịch: Sinh viên thực hiện: Phạm Duy Trọng Page | 68 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường Kết luận: - Qua đồ án em rút việc thiết robot để hoạt động thực tế phức tạp, chi tiết, tỉ mỉ - Đồ án II giúp em hoàn thành công việc thiết kế mô robot phần mềm matlab, sử dụng giao diện GUI để điều khiển mơ robot cách tích hợp code để lập trình - Robot 3DOF em chưa điều khiển hoạt động cách hồn hảo, thơng số PID cài đặt phần mô Simulink chưa phù hợp, nên q trình chạy khơng mong muốn Sinh viên thực hiện: Phạm Duy Trọng Page | 69 Thiết kế mô điều khiển robot GVHD: TS Nguyễn Danh Trường TÀI LIỆU THAM KHẢO Các tài liệu phần mềm Matlab, công cụ Simulink, công cụ Simmechanic Các tài liệu điều lí thuyết điều khiển Sách: Robot cơng nghiệp, Động lực học hệ nhiều vật… Sinh viên thực hiện: Phạm Duy Trọng Page | 70 ... S1S 23 C1C 23 C1S 23 S1C 23 S 23 23 23 23 C1 S1 Page | 29 Thiết kế mô điều khiển robot T3T C1C 23 C1S 23 S1 S1C 23 S1S 23 C1 S 23 C 23 0 (0) T T3 T 1 C1 23 GVHD: TS Nguyễn Danh Trường S1 C1 23 S1 23 23. .. (0) C1 • Với khâu 3: T3 R1.R2 R3 T3 C1C 23 S1C 23 S 23 T2 R3 C1S 23 S1S 23 C 23 S1C 23 Ta có: T3 C1C 23 1 C 23 C1C2 S1C2 S2 C1S2 S1S C2 S1 C3 C1 S3 0 S3 C3 0 S1 C1 C1S 23 S1S 23 23 23 23 Sinh viên thực... khâu 0 • Với khâu 3: R3T C3 S3 S3 C3 R3T R3 0 , R3 1 0 0 S3 C3 C3 S3 0 0 3  Vận tốc góc tương đối khâu 0 Sinh viên thực hiện: Phạm Duy Trọng Page | 27 Thiết kế mô điều khiển robot GVHD: TS Nguyễn