Điều khiển tối ưu năng lượng cánh tay máy chuyển tải

Luận Văn Thạc Só GVHD: Hồ Đắc Lộc Đại Học Quốc Gia Tp.Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA - PHẠM CÔNG THÀNH ĐIỀU KHIỂN TỐI ƯU NĂNG LƯỢNG CÁNH TAY MÁY CHUYỂN TẢI Chun ngành: ĐIỀU KHIỂN HỌC KỸ THUẬT Mã số ngành : 2.05.01 LUẬN VĂN THẠC SĨ SVTH:Phạm Công Thành Trang 59 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc TP.HỒ CHÍ MINH, tháng 10 năm 2005 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH Cán hướng dẫn khoa học: TSKH HỒ ĐẮC LỘC Cán chấm nhận xét 1: ……………………………………… Cán chấm nhận xét 2: ……………………………………… SVTH:Phạm Công Thành Trang 60 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN TRƯỜNG ĐẠI HỌC BÁCH KHOA PHÒNG ĐÀO TẠO SĐH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC Tp HCM, ngày ……tháng ……năm 2005 THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày ……tháng 11 năm 2005 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: Phạm Công Thành Ngày, tháng, năm sinh: 18.6.1978 Chuyên ngành: Điều khiển học kỹ thuật Khóa: 14 Phái: Nam Nơi sinh:Tiền Giang Mã ngành: 2.05.01 MSHV: 01503364 I TÊN ĐỀ TÀI: ĐIỀU KHIỂN TỐI ƯU NĂNG LƯỢNG CÁNH TAY MÁY CHUYỂN TẢI II NHIỆM VỤ VÀ NỘI DUNG: ……………………………………………………………………………………………………………………………………………………………………………… ……………………………………………………………………………………………………………………………………………………………………………… ……………………………………………………………………………………………………………………………………………………………………………… ……………………………………………………………………………………………………………………………………………………………………………… ……………………………………………………………………………………………………………………………………………………………………………… ………………………… III NGÀY GIAO NHIỆM VỤ: ………………… IV NGÀY HOÀN THÀNH NHIỆM VỤ: ………………… SVTH:Phạm Công Thành Trang 61 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc TSKH HỒ ĐẮC LỘC V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: CÁN BỘ HƯỚNG DẪN CHỦ NHIỆM NGÀNH (Ký tên) (Ký tên) BỘMÔN QUẢN LÝ CHUYÊN NGÀNH (Ký tên) Nội dung đề cương luận văn thạc sĩ Hội Đồng Chuyên Ngành thơng qua Ngày ……tháng……năm 2005 PHỊNG ĐÀO TẠO SĐH SVTH:Phạm Công Thành KHOA QUẢN LÝ NGÀNH Trang 62 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc LỜI CẢM ƠN Em xin chân thành cảm ơn Thầy TSHK.Hồ Đắc Lộc tận tình hướng dẫn truyền đạt kiến thức, kinh nghiệm giúp em hoàn thành luận văn Quý thầy cô thuộc môn Điều khiển tự động khoa điện- điện tử tận tình giảng dạy kiến thức cho em suốt khoá học Thành xin cảm ơn tất bạn động viên, giúp đở trao đổi kiến thức giúp Thành trình thực luận văn Cuối xin, xin tỏ lòng biết ơn gia đình , Cha Mẹ người quan tâm ủng hộ giúp hoàn thành luận văn Trân trọng cảm ơn! TP.HCM Tháng 11 năm 2005 Phạm Công Thành SVTH:Phạm Công Thành Trang 63 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc MỤC LỤC Trang bìa Trang Trang Nhiệm vụ luận văn thạc sĩ Lời cảm ơn Toùm tắt luận văn thạc sĩ Mục lục Liệt kê hình PHẦN I TỔNG QUAN VỀ BÀI TOÁN ĐIỀU KHIỂN TỐI ƯU VÀ CÁNH TAY ROBOT TRUYỀN TẢI CHƯƠNG I TỔNG QUAN VỀ CÁNH TAY ROBOT 1.1 Lịch sử hình thành phát triển robot 1.2 Tổng quan điều khiển tối ưu hệ thống phi tuyến 1.2.1 Tổng quan điều khiển tối ưu cho hệ thống phi tuyến động 1.2.2 Tổng quan điều khiển tối ưu cho hệ hybrid 1.2.3 Đặc tính hệ thống hệ thống lai phi tuyến động 1.2.4 Hồi tiếp tuyến tính 1.3 Đặt vấn đề điều khiển tối ưu có ràng buộc ba cánh tay 1.3.1 Mục đích 1.3.2 Phạm vi giới hạn xác định toán 1.4 Hướng giải CHƯƠNG II LÝ THUYẾT TỔNG QUÁT VỀ ĐIỀU KHIỂN TỐI ƯU HỆ PHI TUYẾN 2.1 Sơ lược phương pháp điều khiển tối ưu 2.1.1Phương pháp tónh 2.1.2Phương pháp động 2.1.2.1 Phương pháp biến phân kinh điển 2.1.2.2 Phương pháp quy hoạch động 2.1.2.3 Phương pháp dùng nguyên lý cực đại Pontryagin 2.2 Tìm hiểu kỹ phương pháp điều khiển tối ưu 2.2.1 Điều khiển tối ưu hệ thống rời rạc 2.2.2 Điều khiển tối ưu hệ thống liên tục 2.2.2.1 Phương pháp biến phân 2.2.2.2 Lời giải tổng qt tốn tối ưu hệ liên tục 2.2.3 Bài toán ngõ vào có ràng buộc 2.3 Phương pháp qui hoạch động SVTH:Phạm Công Thành Trang 64 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc 2.3.1 Nguyên lý tối ưu bellman 2.3.2 Hệ thống rời rạc 2.3.3 Hệ thống liên tục 2.4 Thiết kế điều khiển tối ưu phương pháp giải tích PHẦN II XÂY DỰNG MÔ HÌNH TOÁN CỦA CÁNH TAY VÀ THIẾT KẾ BỘ ĐIỀU KHIỂN TỐI ƯU CHƯƠNG I XÂY DỰNG PHƯƠNG TRÌNH ĐỘNG HỌC CỦA CÁNH TAY MÁY KHÂU PHẲNG 1.1 Phương trình động học thuận cho cánh tay máy2 hai khâu phẳng 1.2 Phương trình động học ngược cho cánh tay máy hai khâu phẳng CHƯƠNG II XÂY DỰNG PHƯƠNG TRÌNH ĐỘNG LỰC HỌC ROBOT 2.1 Khai báo thông số 2.1.1 Tham số khớp 2.1.2 Tham số động 2.1.3 Những đối số khớp 2.1.4 Khai báo tham số cánh tay gấp vật 2.2 Vận tốc gia tốc 2.3 Động tay máy 2.4 Áp dụng phương pháp ta tính động lực học cấu tay máy khâu phẳng 2.4.1 Xác định ma trận moment quán tính B(q ) 2.4.2 Xác định ma trận Coriolis C (q, q& ) 2.4.3 Xác định vectơ trọng trường G (q ) 2.4.4 Xác định lực ma sát Fv q& 2.5 Xây dựng hệ phương trình biến trạng thái cánh tay hai khâu phẳng CHƯƠNG III LỰA CHỌN MỘT PHƯƠNG PHÁP ĐỂ ĐIỀU KHIỂN TỐI ƯU BA CÁNH TAY MÁY HAI KHÂU PHẲNG 3.1 Khảo sát tổng quát ba cánh tay robot chuyển tải 3.2 Qúa trình hoat động ba cánh tay robot hai bậc tự 3.3 Thiết kế điều khiển tối ưu phương pháp giải tích 3.4 Dựa vào lý thuyết ta thiết kế luật điều khiển tối ưu cánh tay máy hai khâu phẳng 3.4.1 Tham số khớp nối 3.4.2 Tham số động 3.4.3 Những đối số khớp SVTH:Phạm Công Thành Trang 65 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc 3.4.4 Khai báo tham số cánh tay gắp vật 3.5 Tham số động lực học để tính ma trận quán tính B(q ) cánh tay gấp vật 3.6 Thiết kế điều khiển tối ưu cánh tay không gian công tác PHẦN III KHẢO SÁT CÁNH TAY MÁY TRÊN MATLAB7.01 CHƯƠNG I XÂY DỰNG MÔ HÌNH SIMULINK TRÊN MATLAB7.01 1.1 Nhấn double chuột vào khối inv(B(q))(tau-tau’) ta khối sau 1.1.1 Nhấn double chuột vào khối C(q ,dq)dq _Ma trận Coriolis 1.1.2 Nhấn double chuột vào khối inv(B(q ))_Nghịch đảo ma trận quán tính 1.1.3 Nhấn double chuột vào khối G(q )_Trọng trường 1.1.4 Nhấn double chuột vào khối F_v_Ma sát 1.2 Nhấn double chuột vào khối n(q ,dq) 1.3 Nhấn double chuột vào khối cs(q) 1.4 Nhấn double chuột vào khối k(q)_Động học 1.5 Nhấn double chuột vào khối J(q)_ Jaccobian 1.6 Nhấn double chuột vào khối dJ(q,dq)_Đạo hàm Jaccobian 1.7 Nhấn double chuột vào khối inv(J(q))_Nghịch đảo Jaccobian 1.8 Tín hiệu biến khớp cánh tay lưu vào workspace 1.8.1 Biến không gian khớp cánh tay lưu vào workspace 1.8.1.1 Góc lệch cánh tay 1.8.1.2 Vận tốc góc lệch 1.8.1.3 Tín hiệu điều khiển tối ưu 1.8.2 Tín hiệu biến không gian khớp cánh tay lưu vào workspace 1.8.2.1 Vị trí đầu mút cánh tay 1.8.2.2 Vận tốc đầu mút cánh tay 1.8.2.3 Gia tốc đầu mút cánh tay 1.9 Tín hiệu vào 1.9.1 Hoành độ cánh tay 1.9.1.1 Toạ độ vị trí cánh tay trục hoành 1.9.1.2 Toạ độ vận tốc cánh tay trục hoành 1.9.1.3 Toạ độ gia tốc cánh tay trục hoành 1.9.2 Tung độ cánh tay 1.9.2.1 Toạ độ vị trí cánh tay trục tung 1.9.2.2 Toạ độ vận tốc cánh tay trục tung 1.9.2.3 Toạ độ gia tốc cánh tay trục tung CHƯƠNGII KẾT QUẢ KHẢO SÁT TRÊN MATLAB7.01 SVTH:Phạm Công Thành Trang 66 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc 2.1 Hướng1,thời gian cánh tay 10s chu kỳ lấy mẫu làTc=0.01s, k1=k2=1 Điểm giao vật không nằm đường trung tuyến hai cánh tay.Hàm chi phí J = 9427,7 2.2 Hướng2, thời gian cánh tay 10s chu kỳ lấy mẫu Tc=0.01s Trường hợp1:k1=k2=1 Hàm chi phí J=13939 2.3Hướng3,thời gian cánh tay 10s chu kỳ lấy mẫu Tc=0.01s.Trường hợp1: k1=k2=1 Điểm giao vật không nằm đường trung tuyến hai cánh tay Hàm chi phí J=12381 2.4 Vì cánh tay chưa gấp vật lượng cần cung cấp cho cánh tay nhỏ ta bỏ qua Ta khảo sát tiêu lượng mà cánh tay gấp vật 2.4.1 Thời gian cánh tay 10s chu kỳ lấy mẫu Tc=0.01s 2.4.1.1 Trường hợp1: k1=k2=0.01 2.4.1.2 Trường hợp2: k1=k2=1 2.4.1.3 Trường hợp3: k1=k2=100 2.4.2 Thời gian cánh tay 20s chu kỳ lấy mẫu Tc=0.01s 2.4.2.1 Trường hợp1: k1 = k2 = 0.01 2.4.2 Trường hợp2: k1=k2=1 2.4.2.3 Trường hợp3: k1=k2=100 2.4.3 Thời gian cánh tay 10s, chu kỳ lấy mẫu Tc=0.001s 2.4.4 Thời gian cánh tay 10s chu kỳ lấy mẫu Tc=0.1s 2.5 Nhận xét trường hợp khảo sát PHẦNI V KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN PHẦN V PHỤ LỤC LIỆT KÊ CÁC HÌNH VẼ Phần Hình Nội dung Mô hình tổng quát Robot PhầnI Hình1.1 Hình1.2 Mô hình tổng quát hồi tiếp tuyến tính Hình1.3 Mô hình ba cánh tay robot chuyển tải Hình 2.3.1 Luật điều khiển hồi tiếp trạng thái cực tiểu nhiên liệu PhầnII Hình1.1 Cánh tay máy hai khâu phẳng Hình1.2 Cơ cấu hai khâu phẳng Hình1.3 Phương pháp xây dựng vùng làm việc cấu hai khâu phẳng Hình 1.4 Phát họa phần công tác ba cánh tay SVTH:Phạm Công Thành Trang 5 22 38 40 43 43 Trang 67 Luận Văn Thạc Só PhầnIII GVHD: Hồ Đắc Lộc Hình3.1 Mô hình ba cánh tay robot chuyển tải Hình3.1a Hình3.1a Mô hình ba cánh tay robot chuyển tải theo hướng1 Hình3.1b Mô hình ba cánh tay robot chuyển tải theo hướng2 Hình3.1c Mô hình ba cánh tay robot chuyển tải theo hướng1 Hình3.2 Quỹ đạo chuyển động thời điểm Hình3.3 Mô hình xác cánh tay máy hai khâu Hình1.1 Mô hình simulink cánh tay có điều khiển tối ưu Hình1.2 Mô hình simulink cánh tay Hình1.3 Mô hình simulink C(q ,dq)dq Hình1.4 Mô hình simulink inv(B(q )) Hình1.5 Function invB viết m.file Hình1.6 Mô hình simulink G(q ) Hình1.7 Function F_v viết m.file Hình1.8 Mô hình simulink n(q ,dq) Hình1.9 Mô hình simulink cs(q) Hình1.10 Mô hình simulink k(q) Hình1.11 Mô hình simulink J(q) Hình1.12 Function Jdq viết m.file Hình1.13 Mô hình simulink dJ(q,dq) Hình1.14 Function dJ viết m.file Hình1.15 Mô hình simulink inv(J(q)) Hình1.16 Function invJ_a viết m.file SVTH:Phạm Công Thành Trang 68 60 62 62 63 64 66 77 78 78 78 79 79 80 80 80 81 81 81 82 82 82 83 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % - Executes on button press in chay function chay_Callback(hObject, eventdata, handles) % hObject handle to chay (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) Cap_nhat_thong_so; function thoigian_Callback(hObject, eventdata, handles) % hObject handle to thoigian (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 thoigian as text %str2double(get(hObject,'String')) returns contents of thoigian as a double % - Executes during object creation, after setting all properties function thoigian_CreateFcn(hObject, eventdata, handles) % hObject handle to thoigian (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns SVTH:Phạm Công Thành Trang 189 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc % Hint: edit controls usually have a white background on Windows %See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit19_Callback(hObject, eventdata, handles) % hObject handle to edit19 (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 edit19 as text % str2double(get(hObject,'String')) returns contents of edit19 as a double % - Executes during object creation, after setting all properties function edit19_CreateFcn(hObject, eventdata, handles) % hObject handle to edit19 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else SVTH:Phạm Công Thành Trang 190 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit20_Callback(hObject, eventdata, handles) % hObject handle to edit20 (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 edit20 as text % str2double(get(hObject,'String')) returns contents of edit20 as a double % - Executes during object creation, after setting all properties function edit20_CreateFcn(hObject, eventdata, handles) % hObject handle to edit20 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit17_Callback(hObject, eventdata, handles) % hObject handle to edit17 (see GCBO) SVTH:Phạm Công Thành Trang 191 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc % 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 edit17 as text % str2double(get(hObject,'String')) returns contents of edit17 as a double % - Executes during object creation, after setting all properties function edit17_CreateFcn(hObject, eventdata, handles) % hObject handle to edit17 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit18_Callback(hObject, eventdata, handles) % hObject handle to edit18 (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 edit18 as text % str2double(get(hObject,'String')) returns contents of edit18 as a double SVTH:Phạm Công Thành Trang 192 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc % - Executes during object creation, after setting all properties function edit18_CreateFcn(hObject, eventdata, handles) % hObject handle to edit18 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit23_Callback(hObject, eventdata, handles) % hObject handle to edit23 (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 edit23 as text % str2double(get(hObject,'String')) returns contents of edit23 as a double % - Executes during object creation, after setting all properties function edit23_CreateFcn(hObject, eventdata, handles) % hObject handle to edit23 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called SVTH:Phạm Công Thành Trang 193 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit24_Callback(hObject, eventdata, handles) % hObject handle to edit24 (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 edit24 as text % str2double(get(hObject,'String')) returns contents of edit24 as a double % - Executes during object creation, after setting all properties function edit24_CreateFcn(hObject, eventdata, handles) % hObject handle to edit24 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else SVTH:Phạm Công Thành Trang 194 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit21_Callback(hObject, eventdata, handles) % hObject handle to edit21 (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 edit21 as text % str2double(get(hObject,'String')) returns contents of edit21 as a double % - Executes during object creation, after setting all properties function edit21_CreateFcn(hObject, eventdata, handles) % hObject handle to edit21 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function edit22_Callback(hObject, eventdata, handles) % hObject handle to edit22 (see GCBO) SVTH:Phạm Công Thành Trang 195 Luận Văn Thạc Só GVHD: Hồ Đắc Loäc % 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 edit22 as text % str2double(get(hObject,'String')) returns contents of edit22 as a double % - Executes during object creation, after setting all properties function edit22_CreateFcn(hObject, eventdata, handles) % hObject handle to edit22 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function kr1_Callback(hObject, eventdata, handles) % hObject handle to kr1 (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 kr1 as text % str2double(get(hObject,'String')) returns contents of kr1 as a double % - Executes during object creation, after setting all properties SVTH:Phạm Công Thành Trang 196 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc function kr1_CreateFcn(hObject, eventdata, handles) % hObject handle to kr1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles called empty - handles not created until after all CreateFcns % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function kr2_Callback(hObject, eventdata, handles) % hObject handle to kr2 (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 kr2 as text % str2double(get(hObject,'String')) returns contents of kr2 as a double % - Executes during object creation, after setting all properties function kr2_CreateFcn(hObject, eventdata, handles) % hObject handle to kr2 (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 SVTH:Phạm Công Thành Trang 197 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end ############################################################### ######## 1.1 Cập nhật thông số % Khoi tao cac bien de hoat hinh cho canh tay may hai khau phang %clear; global m_1 m_2 m_pl a_1 a_2 global ml_1 ml_2 global pxy_f %Nap cac thong so tu giao dien dieu khien cho canh tay may mang tai handles = guihandles(handles.SCARA_GUI); %Khoang cach giua cac truc %a_1 = 1; %a_2 = 1; assignin('base','a_1',str2double(get(handles.a1,'string'))); assignin('base','a_2',str2double(get(handles.a2,'string'))); %Khoi luong cac %m_l1 = 4; %m_l2 = 2; %assignin('base','m_l1',str2double(get(handles.ml1,'string'))); %assignin('base','m_l2',str2double(get(handles.ml2,'string'))); SVTH:Phạm Công Thành Trang 198 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc %Khoi luong dong co %m_m1 %m_m2 %assignin('base','m_m1',str2double(get(handles.mm1,'string'))); %assignin('base','m_m2',str2double(get(handles.mm2,'string'))); %Khoi luong cua vat gap len assignin('base','m_pl',str2double(get(handles.mpl,'string'))); %Khoi luong canh tay %m_1= m_m2 + m_l1; %m_2= m_l2; luc chua gap vat %m_2= m_l2+ m_pl; luc gap vat %assignin('base','m_1',str2double(get(handles.m1,'string'))); %assignin('base','m_2',str2double(get(handles.m2,'string'))); %Gia toc truong %g = 9.81; assignin('base','g',9.81) %Cac thong so cua bo dieu khien toi uu %k_1 = 1; %k_2 = 1; assignin('base','k_1',str2double(get(handles.K1,'string'))); assignin('base','k_2',str2double(get(handles.K2,'string'))); %Cac he so %k_r1 = 1; %k_r2 = 1; SVTH:Phạm Công Thành Trang 199 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc assignin('base','k_r1',str2double(get(handles.Kr1,'string'))); assignin('base','k_r2',str2double(get(handles.Kr2,'string'))); %Vi tri khoi tao cua dau mut %gia tri x ban dau -1 %gia tri y ban dau 0.25 %pxy_i = [-1;0.25]; assignin('base','pxy_i',[str2double(get(handles.xi,'string')) ;str2double(get(handles.yi,'string'))]); %Cau hinh khop ban dau %qxy_i = inv_k2u(a_1,a_2,pxy_i); assignin('base','qxy_i',inv_k2u(str2double(get(handles.a1,'string')), str2double(get(handles.a2,'string')), [str2double(get(handles.xi,'string')); str2double(get(handles.yi,'string'))])); %Vi tri cuoi mong muon cua dau mut %gia tri x cuoi %gia tri y cuoi 0.75 %gia tri z cuoi 0.5 %pxy_f = [1;0.75]; %pz_f = 0.5; assignin('base','pxy_f',[str2double(get(handles.xf,'string')) ;str2double(get(handles.yf,'string'))]); %Phat sinh quy dao dat Dat_quy_dao; SVTH:Phạm Công Thành Trang 200 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% 1.2 Đặt quỹ đạo % Phat sinh quy dao dat cho canh tay may hai khau phang global pxy_f pz_f handles = guihandles(handles.SCARA_GUI); %Tao vector thoi gian %t_d = 10; %Thoi gian keo dai, cung la thoi gian mo phong assignin('base','t_d',str2double(get(handles.td,'string'))); %Tc = 0.01; %Chu ky lay mau assignin('base','Tc',0.01); %T = (0:Tc:t_d)'; %Vector thoi gian assignin('base','T',(0:0.01:str2double(get(handles.td,'string')))'); %w = size(T,1); w = size((0:0.01:str2double(get(handles.td,'string')))',1); a = ones(w,2); %Ma tran chua thong tin duong dan den diem cuoi %Tao gia tri dat cuoi for m = 1:w c(m)=evalin('base','pxy_f(1)'); %Gia tri x cuoi a1=c'; end for n=1:w b(n)=evalin('base','pxy_f(2)'); %Gia tri y cuoi a2=b'; end SVTH:Phạm Công Thành Trang 201 Luận Văn Thạc Só GVHD: Hồ Đắc Loäc a=[a1 a2]; %assignin('base','a',a); %Phat sinh quy dao dat for j=1:2 for i=1:w q_d(i,j)=a(i,j); %Vi tri end end dq_d=zeros(w,2); %Van toc ddq_d=zeros(w,2); %Gia toc assignin('base','q_d',q_d); assignin('base','dq_d',dq_d); assignin('base','ddq_d',ddq_d); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% CE:control Event DE:Disturbance event EL-DEQ : Euler lagrange Differential Equation TE: Time Event Sách tham khảo : -Robotics tác giả :K.S.FU; P.C.Gonzalez;C.S.G.Lee -A General framework for Hybrid H ∞ -Control tác giả ANNAKARIN CHISTIANSSON -Nonlinear Hybrid Dynamical Systems Của tác giả Martin Buss,Markus Glocker ,Michael Hardt, Oskar Von Stryk SVTH:Phạm Công Thành Trang 202 Luận Văn Thạc Só GVHD: Hồ Đắc Lộc -Hybrid systems control tác giả Panos J.Antsaklis and Xenofon D.Koutsoukos SVTH:Phạm Công Thành Trang 203 ... xong cánh tay hai bắt đầu di chuyển lúc cánh tay ba đứng yên Cánh tay gấp vật di chuyển tới điểm đường trung tuyến cánh tay cánh tay hai cánh tay hai vừa tới điểm lúc cánh tay hai cánh tay thắng... ba cánh tay máy hệ thống lai,với thời gian cánh liên tục cánh tay chuyển vật cho từ cánh tay sang cánh tay có kiện rời rạc lúc chuyển Khi cánh tay chưa gấp vật di chuyển lại để gấp vật cánh tay. .. động) cánh tay máy chuyển tải Năng lượng tiêu hao cánh tay nhỏ 1.3.2 Phạm vi giới hạn xác định toán • Ba cánh tay máy chuyển động mặt phẳng xoy Chúng ta điều khiển phối hợp với cho lượng để ba cánh