TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN BÀI TẬP LỚN ROBOT NHÓM 4 ROBOT PLANAR Giáo viên hướng dẫn PSG TS Nguyễn Phạm Thục Anh Sinh viên MSSV Lớp Trần Trung Hiếu 20101527 ĐK&TĐH4 K55 Đỗ Đình Khải 201[.]
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN BÀI TẬP LỚN ROBOT NHÓM 4: ROBOT PLANAR Giáo viên hướng dẫn: PSG.TS Nguyễn Phạm Thục Anh Sinh viên Trần Trung Hiếu Đỗ Đình Khải Nguyễn Văn Khánh Lê Quang Huy Bùi Đình Khoa MSSV 20101527 20111692 20111689 20111652 20109547 - Hà Nội, tháng 5/2015 Lớp ĐK&TĐH4-K55 ĐK&TĐH1-K56 ĐK&TĐH2-K56 ĐK&TĐH7-K56 CN-ĐK&TĐH2-K55 Hà Nội, tháng 5/2015 Hà Nội tháng 02/2002 `1Mục lục Phần I.Giới thiệu robot 1.Giới thiệu chung robot, lịch sử phát triển robot 2 Một số hình ảnh robot thực tế .2 Cấu trúc robot 4.Ứng dụng robot Robot Planar dof .5 a.Có cầu hình vẽ: b.Ứng dụng robot planar Phần II: Xây dựng phần mềm tính tốn động học robot, ma trận Jacobi hiển thị giao diện matlab I.Động học thuận vị trí 1.Thiết kế hệ tọa độ cho robot 2.Xây dựng toán động học thuận robot II.Động học ngược vị trí 11 1.Xây dựng động học ngược vị trí 11 2.Phương trình động học ngược vị trí robot 12 III.Ma trận Jacobi 13 1.Ma trận J 13 2.Ma trận JH 13 IV: Thiết kế hiển thị giao diện matlab 15 1.Giới thiệu toolbox robotics 15 Giao diện 15 3.Code giao diện 17 Phần I.Giới thiệu robot 1.Giới thiệu chung robot, lịch sử phát triển robot Rôbốt hay người máy loại máy thực cơng việc cách tự động điều khiển máy tính vi mạch điện tử lập trình Năm 1956, Engelberger , công ty sản xuất robot Unimation Năm 1969, Victor Scheinman , Stanford, trở thành chuẩn mực ảnh hưởng nhiều Từ năm 1966 năm 1972, Shakey, tảng có ảnh hưởng to lớn đến trí thơng minh nhân tạo khoa học robot ngày Năm 1974, Robot Sigma sử dụng công nghiệp lắp ráp – ứng dụng robot dây chuyền lắp ráp Một số hình ảnh robot thực tế Cấu trúc robot Phần công tác: tác động vào vật Cơ cấu chấp hành: tạọ chuyển động cho khâu tay máy Hệ thống cảm biến: sensor thiết bị chuyển đổi; thân+ mơi trường Điều khiển: dùng máy tính để giám sát điều khiển hoạt động robot 4.Ứng dụng robot - Góp phần nâng cao chất lượng dây chuyền công nghệ,giảm giá thành,nâng cao chất lượng sản phẩm tang suất lao động - Thay người cơng việc mệt nhọc,địi hỏi sựu tỉ mỉ xác,mơi trường lao động khắc nghiệt - Trong ngành khí robot sử dụng cơng nghệ đúc,công nghệ hàn,cắt kim loại,tháo lắp vận chuyển phôi,vận chuyển sản phẩm,… - Robot áp dụng rộng rãi ngành lớn y học,quốc phịng,cơng nghiệp ngun tử chinh phục vũ trụ,… -Tuy có nhiều ứng dụng lớn sống ngày robot có nhược điểm lớn hoạt động khơng linh hoạt,khi gặp cố dây chuyền sản phẩm làm ngừng tồn hệ thống Do robot hoạt động dựa vào dám sát điều khiển người Robot Planar dof a.Có cầu hình vẽ: Đây robot có bậc tự , khớp có nối bao gồm bệ đế Ưu điểm cấu trúc đơn giản, dễ sử dụng, tính tốn thơng số dễ dàng, robot dễ chế tạo.Tuy nhiên robot hoạt động mặt phẳng nên robot ứng dụng thực tế Thông số robot planar là: Chiều dài nối, khối lượng nối,góc quay tối đa khớp quay,cánh tay máy b.Ứng dụng robot planar Phục hồi chức tay, với tên gọi Planar MEDARM.Robot 3DOF khung mà cung cấp đo lường độc lập kiểm soát chuyển động vai, khuỷu tay , cổ tay mặt phẳng nằm ngang.Toàn chế nằm bên cánh tay người dùng, tất khớp xương kích thích hệ thống cáp ổ đĩa Planar vận chuyển cách độc lập cho phép ứng dụng nhiều loại tải.Ví dụ robot định lượng khả vấn đề để bù nhiễu loạn áp dụng cho đầu nối cánh tay Phần II: Xây dựng phần mềm tính tốn động học robot, ma trận Jacobi hiển thị giao diện matlab Cấu hình robot : I.Động học thuận vị trí 1.Thiết kế hệ tọa độ cho robot a.Tham số nối khớp Độ dài nối a1 , a2 , a3 (với a1 = 5(m), a2 = 3(m), a3 = 2(m) ) Xác định bảng thơng số D-H Từ hình vẽ robot ta xác định thông số sau: + Độ dài pháp tuyến chung khớp khớp a1 + Góc chéo trục khớp khớp + Đây khớp quay lên khoảng cách đo dọc trục khớp động từ đường vng góc chung khớp động khớp động tới đường vng góc chung khớp động 1và gốc robot d1 = Tương tự lập luận ta có d = 0, d3 = + Góc hai đường vng góc chung q1 Tương tự với khớp khớp ta có : + Độ dài pháp tuyến chung khớp khớp a2 + Góc chéo trục khớp khớp + Góc hai đường vng góc chung q2 Tương tự với khớp tay robot ta có: + Độ dài pháp tuyến chung khớp tay robot a3 + Góc chéo trục khớp tay robot + Góc hai đường vng góc chung q3 Vậy biến khớp robot q1 , q2 , q3 2.Xây dựng toán động học thuận robot Phương trình động học thuận robot phương trình biểu diễn quan hệ vị trí tay robot thơng qua vị trí biến khớp.Các biến khớp góc quay cảu khớp quay,và độ dịch chuyển khớp tịnh tiến khớp tịnh tiến Căn vào phương trình động học thuận robot biết vị trí biến khớp ta xác định hướng vị trí cánh tay robot Căn vào hệ tọa độ thơng số ta có bảng D-H sau : Khâu qi i di q1 a1 q2 a2 q3 a3 set(handles.slQ2,'value',temp); else set(handles.txtStatus,'string','Error data!!'); end handles.q2 = (temp-0.5)*2*pi; guidata(hObject, handles); else temp = str2double(get(hObject,'string'))/(2*pi)+0.5; if and(temp=0) set(handles.slQ2,'value',temp); else set(handles.txtStatus,'string','Error data!!'); end handles.q2 = (temp-0.5)*2*pi; guidata(hObject, handles); end plotRobot(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3); %Ve robot len figure handles.str = sprintf('%s\n\n[ px py ] %s',handles.str,mat2str(calcToaDo(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3))); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); = % - Executes during object creation, after setting all properties function txtQ2_CreateFcn(hObject, eventdata, handles) % hObject handle to txtQ2 (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 txtQ3_Callback(hObject, eventdata, handles) % hObject handle to txtQ3 (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 txtQ3 as text % str2double(get(hObject,'String')) returns contents of txtQ3 as a double a = get(handles.checkbox3,'value'); set(handles.txtStatus,'string','OK!'); if a == temp = str2double(get(hObject,'string'))/360+0.5; if and(temp=0) set(handles.slQ3,'value',temp); else set(handles.txtStatus,'string','Error data!!'); end handles.q3 = (temp-0.5)*2*pi; guidata(hObject, handles); else 23 temp = str2double(get(hObject,'string'))/(2*pi)+0.5; if and(temp=0) set(handles.slQ3,'value',temp); else set(handles.txtStatus,'string','Error data!!'); end handles.q3 = (temp-0.5)*2*pi; guidata(hObject, handles); end plotRobot(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3); %Ve robot len figure handles.str = sprintf('%s\n\n[ px py ] %s',handles.str,mat2str(calcToaDo(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3))); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); = % - Executes during object creation, after setting all properties function txtQ3_CreateFcn(hObject, eventdata, handles) % hObject handle to txtQ3 (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 pushbutton2 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) % - Executes on button press in pushbutton3 function pushbutton3_Callback(hObject, eventdata, handles) % hObject handle to pushbutton3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) function txtX_Callback(hObject, eventdata, handles) % hObject handle to txtX (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 txtX as text % str2double(get(hObject,'String')) returns contents of txtX as a double handles.px = str2num(get(hObject,'string')); guidata(hObject, handles); % - Executes during object creation, after setting all properties function txtX_CreateFcn(hObject, eventdata, handles) 24 % hObject handle to txtX (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 txtY_Callback(hObject, eventdata, handles) % hObject handle to txtY (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 txtY as text % str2double(get(hObject,'String')) returns contents of txtY as a double handles.py = str2num(get(hObject,'string')); guidata(hObject, handles); % - Executes during object creation, after setting all properties function txtY_CreateFcn(hObject, eventdata, handles) % hObject handle to txtY (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 edit10_Callback(hObject, eventdata, handles) % hObject handle to edit10 (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 edit10 as text % str2double(get(hObject,'String')) returns contents of edit10 as a double % - Executes during object creation, after setting all properties function edit10_CreateFcn(hObject, eventdata, handles) % hObject handle to edit10 (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 25 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function txtInitA1_Callback(hObject, eventdata, handles) % hObject handle to txtInitA1 (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 txtInitA1 as text % str2double(get(hObject,'String')) returns contents of txtInitA1 as a double handles.a1 = str2num(get(handles.txtInitA1,'string')); guidata(hObject, handles); % - Executes during object creation, after setting all properties function txtInitA1_CreateFcn(hObject, eventdata, handles) % hObject handle to txtInitA1 (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 txtInitA2_Callback(hObject, eventdata, handles) % hObject handle to txtInitA2 (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 txtInitA2 as text % str2double(get(hObject,'String')) returns contents of txtInitA2 as a double handles.a2 = str2num(get(handles.txtInitA2,'string')); guidata(hObject, handles); % - Executes during object creation, after setting all properties function txtInitA2_CreateFcn(hObject, eventdata, handles) % hObject handle to txtInitA2 (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 btnWatchDH 26 function btnWatchDH_Callback(hObject, eventdata, handles) % hObject handle to btnWatchDH (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) str= get(handles.ppmDH,'string'); val= get(handles.ppmDH,'value'); switch str{val} case 'A1' if( get(handles.rbtnDH,'value') == 0) tempStr = mat2str(calcA1(handles.q1,handles.a1)); handles.str = sprintf('%s\nA1 = %s',handles.str,tempStr); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n%s',handles.str,handles.strA1); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end case 'A2' if( get(handles.rbtnDH,'value') == 0) tempStr = mat2str(calcA2(handles.q2,handles.a2)); handles.str = sprintf('%s\nA2 = %s',handles.str,tempStr); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n%s',handles.str,handles.strA2); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end case 'A3' if( get(handles.rbtnDH,'value') == 0) tempStr = mat2str(calcA3(handles.q3,handles.a3)); handles.str = sprintf('%s\nA1 = %s',handles.str,tempStr); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n%s',handles.str,handles.strA3); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end case 'table_DH' end % - Executes on selection change in ppmDH function ppmDH_Callback(hObject, eventdata, handles) % hObject handle to ppmDH (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: contents = cellstr(get(hObject,'String')) returns ppmDH contents as cell array % contents{get(hObject,'Value')} returns selected item from ppmDH % - Executes during object creation, after setting all properties 27 function ppmDH_CreateFcn(hObject, eventdata, handles) % hObject handle to ppmDH (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: popupmenu 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 rbtnDH function rbtnDH_Callback(hObject, eventdata, handles) % hObject handle to rbtnDH (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hint: get(hObject,'Value') returns toggle state of rbtnDH % - Executes on selection change in ppmTrans function ppmTrans_Callback(hObject, eventdata, handles) % hObject handle to ppmTrans (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: contents = cellstr(get(hObject,'String')) returns ppmTrans contents as cell array % contents{get(hObject,'Value')} returns selected item from ppmTrans % - Executes during object creation, after setting all properties function ppmTrans_CreateFcn(hObject, eventdata, handles) % hObject handle to ppmTrans (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: popupmenu 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 rbtnTrans function rbtnTrans_Callback(hObject, eventdata, handles) % hObject handle to rbtnTrans (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hint: get(hObject,'Value') returns toggle state of rbtnTrans 28 % - Executes on selection change in ppmJacoby function ppmJacoby_Callback(hObject, eventdata, handles) % hObject handle to ppmJacoby (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: contents = cellstr(get(hObject,'String')) returns ppmJacoby contents as cell array % contents{get(hObject,'Value')} returns selected item from ppmJacoby % - Executes during object creation, after setting all properties function ppmJacoby_CreateFcn(hObject, eventdata, handles) % hObject handle to ppmJacoby (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: popupmenu 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 rbtnJacoby function rbtnJacoby_Callback(hObject, eventdata, handles) % hObject handle to rbtnJacoby (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hint: get(hObject,'Value') returns toggle state of rbtnJacoby function txtInitA3_Callback(hObject, eventdata, handles) % hObject handle to txtInitA3 (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 txtInitA3 as text % str2double(get(hObject,'String')) returns contents of txtInitA3 as a double handles.a3 = str2num(get(handles.txtInitA3,'string')); guidata(hObject, handles); % - Executes during object creation, after setting all properties function txtInitA3_CreateFcn(hObject, eventdata, handles) % hObject handle to txtInitA3 (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'); 29 end % - Executes on button press in btnWatchTrans function btnWatchTrans_Callback(hObject, eventdata, handles) % hObject handle to btnWatchTrans (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) str = get(handles.ppmTrans,'string'); val = get(handles.ppmTrans,'value'); switch str{val} case 'T03' if( get(handles.rbtnTrans,'value') == ) tempStr = mat2str(calcT_03(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3)); handles.str = sprintf('%s\n\nT_03 = %s',handles.str,tempStr); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n%s',handles.str,handles.strT_03); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end case 'T13' if( get(handles.rbtnTrans,'value') == ) tempStr = mat2str(calcT_13(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3)); handles.str = sprintf('%s\n\nT_13 = %s',handles.str,tempStr); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n\n%s',handles.str,handles.strT_13); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end case 'T23' if( get(handles.rbtnTrans,'value') == ) tempStr = mat2str(calcT_23(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3)); handles.str = sprintf('%s\n\nT_23 = %s',handles.str,tempStr); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n\n%s',handles.str,handles.strT_23); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end end % - Executes on button press in btnWatchJacoby function btnWatchJacoby_Callback(hObject, eventdata, handles) % hObject handle to btnWatchJacoby (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) str = get(handles.ppmJacoby,'string'); val = get(handles.ppmJacoby,'value'); switch str{val} case 'Jacoby' %Hien thi ma tran Jacoby 30 if( get(handles.rbtnJacoby,'value') == ) tempStr = mat2str(calcJacoby(handles.q1,handles.q2,handles.q3,handles.a1,handles.a2,handles.a3)); handles.str = sprintf('%s\n\nJACOBY = %s',handles.str,tempStr);%Hien thi ma tran jacoby dang so guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n\n%s',handles.str,handles.strJacoby);%Hien thi ma tran jacoby dang cong thuc guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end case 'q' %Hien thi cac thong tin [q1 q2 q3] if( get(handles.rbtnJacoby,'value') == ) [q11 q12 q13 status] = calcNguoc(handles.a1,handles.a2,handles.a3,handles.px,handles.py); if status == handles.str = sprintf('%s\n\n[q1 q2 q3 ] = [%f %f %f]',handles.str,q11,q12,q13); else handles.str = sprintf('%s\n\n[q1 q2 q3] Khong co gia tri!',handles.str); end guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else handles.str = sprintf('%s\n\n%s\n\n%s\n\n%s\n\n%s\n\n%s',handles.str,handles.strPwx,handles.strPwy,handles.strQ1,handles.strQ 2,handles.strQ3); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end end function txtContent_Callback(hObject, eventdata, handles) % hObject handle to txtContent (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 txtContent as text % str2double(get(hObject,'String')) returns contents of txtContent as a double % - Executes during object creation, after setting all properties function txtContent_CreateFcn(hObject, eventdata, handles) % hObject handle to txtContent (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 btnStart function btnStart_Callback(hObject, eventdata, handles) 31 % hObject handle to btnStart (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) plotRobot(handles.initQ1,handles.initQ2,handles.initQ3,handles.initA1,handles.initA2,handles.initA3); handles.str = sprintf('\n%s\n\n%s\n\n%s\n\n%s\n\n%s\n\n%s\n\n%s\n\n%s',handles.strA1,handles.strA2,handles.strA3,handles.str T_03,handles.strT_13,handles.strT_23,handles.strJacoby,handles.strToaDo); guidata(hObject, handles); handles.str = sprintf('%s\n\n _DONG HOC NGUOC VI TRI \n\n%s\n\n%s\n\n%s\n\n%sn\n%s',handles.str,handles.strPwx,handles.strPwy,handles.strQ1,handles.st rQ2,handles.strQ3); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); % - Executes on button press in btnStop function btnStop_Callback(hObject, eventdata, handles) % hObject handle to btnStop (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) %Khoi tao lai cac gia tri set(handles.slQ1,'value',0.5); set(handles.slQ2,'value',0.5); set(handles.slQ3,'value',0.5); set(handles.txtQ1,'string','0'); set(handles.txtQ2,'string','0'); set(handles.txtQ3,'string','0'); %Xoa du lieu txtContent handles.str = ''; guidata(hObject, handles); set(handles.txtContent,'string',handles.str); % - Executes on button press in btnRun function btnRun_Callback(hObject, eventdata, handles) % hObject handle to btnRun (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) if get(handles.cbQuiDao,'value') == [q11 q12 q13 status] = calcNguoc(handles.a1,handles.a2,handles.a3,handles.px,handles.py); if status == plotRobot(q11,q12,q13,handles.a1,handles.a2,handles.a3); set(handles.txtStatus,'string','OK!'); handles.str = sprintf('%s\n\n[p1 p2 p3 ] = [%f %f %f]',handles.str,q11,q12,q13); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); else set(handles.txtStatus,'string','Vi tri nhap khong ton tai!!'); handles.str = sprintf('%s\n\nVi tri nhap khong ton tai!!',handles.str); guidata(hObject, handles); set(handles.txtContent,'string',handles.str); end else paint(handles.a1,handles.a2,handles.a3,handles.px,handles.py); end % - Executes on button press in checkbox1 function checkbox1_Callback(hObject, eventdata, handles) 32 % hObject handle to checkbox1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hint: get(hObject,'Value') returns toggle state of checkbox1 a = get(hObject,'Value'); if a == set(hObject, 'string','Deg'); else set(hObject, 'string','Rad'); end % - Executes on button press in checkbox2 function checkbox2_Callback(hObject, eventdata, handles) % hObject handle to checkbox2 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hint: get(hObject,'Value') returns toggle state of checkbox2 a = get(hObject,'Value'); if a == set(hObject, 'string','Deg'); else set(hObject, 'string','Rad'); end % - Executes on button press in checkbox3 function checkbox3_Callback(hObject, eventdata, handles) % hObject handle to checkbox3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hint: get(hObject,'Value') returns toggle state of checkbox3 a = get(hObject,'Value'); if a == set(hObject, 'string','Deg'); else set(hObject, 'string','Rad'); end % - Executes during object creation, after setting all properties function txtStatus_CreateFcn(hObject, eventdata, handles) % hObject handle to txtStatus (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called function paint(a11,a21,a31, px,py) [q11 q12 q13 status] = calcNguoc(a11,a21,a31,px,py); if( status == ) if q11 < q11 = q11 + 2*pi; end startup_rvc % Kh?i ??ng Robotics Toolbox (file strartup_rvc c?a ROBOTICS TOOLBOX ph?i ?ang n?m ? th? % m?c hi?n th?i (Current Directory c?a MATLAB 33 % Khai báo symbolic variables ?i?u ki?n c?a bi?n syms p1 p2 p3 b1 b2 b3 t % q1, q2, q3 bi?n kh?p % a1, a2, a3 ?? dài % dq1, dq2, dq3 ??o hàm c?a q1, q2, q3 % m1, m2, m3 kh?i l??ng % t : bi?n th?i gian % g : gia t?c tr?ng tr??ng %q = [q1;q2;q3]; % Vector t?a ?? suy r?ng q %dq = [dq1;dq2;dq3]; % Vector v?n t?c dài % Nh?p ma tr?n D-H A_01=[cos(p1) -sin(p1) b1*cos(p1);sin(p1) cos(p1) b1*sin(p1);0 0;0 0 1]; A_12=[cos(p2) -sin(p2) b2*cos(p2);sin(p2) cos(p2) b2*sin(p2);0 0;0 0 1]; A_23=[cos(p3) -sin(p3) b3*cos(p3);sin(p3) cos(p3) b3*sin(p3);0 0;0 0 1]; % Tính mb tr?n truy?n A_03=A_01*A_12*A_23; A_03 = simplify(A_03); % T?o mô h́ nh Robot Robotics toolbox L2(1)=Link([0,0,a11,0,0]); % L?nh Link t?o m?t khâu c?a Robot L2(2)=Link([0,0,a21,0,0]); L2(3)=Link([0,0,a31,0,0]); rob2=SerialLink(L2); % L?nh SerialLink(L) t?o m?t robot n?i ti?p g?m khâu c?a L % Gi?i toán ??ng h?c thu?n % T́ m vi trí tính v?n t?c dài c?a khâu thao tác rE = A_03(1:3,4) ; % Vector t?a ?? khâu thao tác % Thay so bai toan dong hoc thuan q11/3 q12*3/q11 q13*3/q11 sub_rE = simplify(subs(rE,{p1 p2 p3 b1 b2 b3},{3*t (q12*3/q11)*t (q13*3/q11)*t a11 a21 a31})); time=0:0.02:q11/3;%(atan(py/px)/6); num_rE=zeros(3,length(time)); for j=1:length(time) num_rE(:,j) = subs(sub_rE,t,time(j)); end figure(2) clf title('Quy dao cua khau tac dong cuoi bai toan thuan') hold on grid on axis([-(a11+a21+a31), (a11+a21+a31), -(a11+a21+a31), (a11+a21+a31) ,-(a11+a21+a31)/2, (a11+a21+a31)/2]) for j=1:length(time) plot3(num_rE(1,j),num_rE(2,j),num_rE(3,j),'b+'); % V? qu? ??o chuy?n ??ng b?ng MATLAB plot(rob2,[3*time(j),(q12*3/q11)*time(j),(q13*3/q11)*time(j)]); % V? h́ nh ?nh chuy?n ??ng 3D c?a Robot theo qu? ??o pause(1/30) % Dùng lenh pause ?? t?o c?m giác gi?ng nh? m?t ?o?n phim end end 34 %Noi dung :Ham tinh ma tran A1 %Tham bien : q1 : goc q1 % a1 : dai noi %Tra ve : Ma tran A1(4x4) function [A1] = calcA1(q1,a1) A1 = [cos(q1) -sin(q1) a1*cos(q1);sin(q1) cos(q1) a1*sin(q1);0 0;0 0 1]; A1 = round(A1*10000)/10000; %Noi dung :Ham tinh ma tran A2 %Tham bien : q2 : goc q2 % a2 : dai noi %Tra ve : Ma tran A2(4x4) function [A2] = calcA2(q2,a2) A2 = [cos(q2) -sin(q2) a2*cos(q2);sin(q2) cos(q2) a2*sin(q2);0 0;0 0 1]; A2 = round(A2*10000)/10000; %Noi dung :Ham tinh ma tran A3 %Tham bien : q3 : goc q3 % a3 : dai noi %Tra ve : Ma tran A3(4x4) function [A3] = calcA3(q3,a3) A3 = [cos(q3) -sin(q3) a3*cos(q3);sin(q3) cos(q3) a3*sin(q3);0 0;0 0 1]; A3 = round(A3*10000)/10000; %Noi dung : Ham tinh ma tran JACOBY %Tham bien : q1,2,3 : goc q1,2,3 % a1,2,3 : dai noi 1,2,3 %Tra ve : Ma tran J(3x3) function [J] = calcJacoby(q1,q2,q3,a1,a2,a3) J = [-a2*sin(q1 + q2)-a1*sin(q1)-a3*sin(q1 + q2 + q3), - a2*sin(q1 + q2)-a3*sin(q1 + q2 + q3), -a3*sin(q1 + q2 + q3); a2*cos(q1 + q2)+a1*cos(q1)+a3*cos(q1 + q2 + q3), a2*cos(q1 + q2)+a3*cos(q1 + q2 + q3), a3*cos(q1 + q2 + q3); 0, 0, 0]; J = round(J*10000)/10000; %Noi dung : Ham tinh ma tran T_03 %Tham bien : q1,2,3 : goc q1,2,3 % a1,2,3 : dai noi 1,2,3 %Tra ve : Ma tran T_03(4x4) function [T_03] = calcT_03(q1,q2,q3,a1,a2,a3) T_03 = [ cos(q1 + q2 + q3), -sin(q1 + q2 + q3), 0, a2*cos(q1 + q2) + a1*cos(q1) + a3*cos(q1 + q2 + q3); sin(q1 + q2 + q3), cos(q1 + q2 + q3), 0, a2*sin(q1 + q2) + a1*sin(q1) + a3*sin(q1 + q2 + q3); 0, 0, 1, ;0, 0, 0, 1]; T_03 = round(T_03*10000)/10000; %Noi dung : Ham tinh ma tran T_13 %Tham bien : q1,2,3 : goc q1,2,3 % a1,2,3 : dai noi 1,2,3 %Tra ve : Ma tran T_13(4x4) function [T_13] = calcT_13(q1,q2,q3,a1,a2,a3) T_13 = [ cos(q1 + q2), -sin(q1 + q2), 0, a2*cos(q1 + q2) + a1*cos(q1); sin(q1 + q2), cos(q1 + q2), 0, a2*sin(q1 + q2) + a1*sin(q1); 0, 0, 1, 0; 0, 0, 0, 1]; T_13 = round(T_13*10000)/10000; %Noi dung : Ham tinh ma tran T_23 %Tham bien : q1,2,3 : goc q1,2,3 % a1,2,3 : dai noi 1,2,3 %Tra ve : Ma tran T_23(4x4) function [T_23] = calcT_23(q1,q2,q3,a1,a2,a3) T_23 = calcA1(q1,a1); T_23 = round(T_23*10000)/10000; %Noi dung : Tinh toan dong hoc nguoc vi tri %Tham bien : a1,a2,a3 35 % px,py : % status(1/0-co nghiem/vo nghiem) %Tra ve : q1,q2,q3 function [q1 q2 q3 status] = calcNguoc(a1,a2,a3,px,py) status = 0; q1 = 0;q2 = 0; q3 = 0; %Khoi tao if sqrt(px^2+py^2) pi q2 = q2 - 2*pi*sign(q1); end if abs(q3) > pi q3 = q3 - 2*pi*sign(q1); end status = 1; %Kiem tra xem phuong trinh co nghiem hay kko? if temp >= pi status = 0; %Vo nghiem end end %Noi dung : Ham tinh toa (px,py) %Tham bien : q1,2,3 : goc q1,2,3 % a1,2,3 : dai noi 1,2,3 %Tra ve : (px,py) function [ q ] = calcToaDo(q1,q2,q3,a1,a2,a3) px = a1*cos(q1) + a2*cos(q1+q2) + a3*cos(q1+q2+q3); py = a1*sin(q1) + a2*sin(q1+q2) + a3*sin(q1+q2+q3); q = [ px py ]; q = round(q*1000)/1000; %Noi dung : Ve thi cho phan dong hoc thuan %Tham bien : q1,2,3 : goc q1,2,3 % a1,2,3 : dai noi 1,2,3 %Tra ve : Khong 36 function plotRobot(q11,q12,q13,a11,a12,a13) %startup_rcv; syms q1 q2 q3 a1 a2 a3 dq1 dq2 dq3 m1 m2 m3 t g; figure(1); clf; title('Khao sat Robot Planar 3DOF') L(1)=Link([0,0,a11,0,0]); % L?nh Link t?o m?t khâu c?a Robot L(2)=Link([0,0,a12,0,0]); L(3)=Link([0,0,a13,0,0]); %px = a1*cos(q1) + a2*cos(q1+q2) + a3*cos(q1+q2+q3); %py = a1*sin(q1) + a2*sin(q1+q2) + a3*sin(q1+q2+q3); rob=SerialLink(L); plot(rob,[q11,q12,q13]); function cbQuiDao_Callback(hObject, eventdata, handles) % hObject handle to cbQuiDao (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hint: get(hObject,'Value') returns toggle state of cbQuiDao 37 ... thiệu robot 1.Giới thiệu chung robot, lịch sử phát triển robot 2 Một số hình ảnh robot thực tế .2 Cấu trúc robot 4.Ứng dụng robot Robot. .. ứng dụng lớn sống ngày robot có nhược điểm lớn hoạt động khơng linh hoạt,khi gặp cố dây chuyền sản phẩm làm ngừng tồn hệ thống Do robot hoạt động dựa vào dám sát điều khiển người Robot Planar dof... dụng thực tế Thông số robot planar là: Chiều dài nối, khối lượng nối,góc quay tối đa khớp quay,cánh tay máy b.Ứng dụng robot planar Phục hồi chức tay, với tên gọi Planar MEDARM .Robot 3DOF khung mà