Luận văn điều khiển phi tuyến hệ agv

i LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi Các số liệu, kết nêu Luận văn trung thực chưa công bố công trình khác Tơi xin cam đoan giúp đỡ cho việc thực Luận văn cảm ơn thơng tin trích dẫn Luận văn rõ nguồn gốc Học viên thực Luận văn (Ký ghi rõ họ tên) Phạm Quốc Thiện ii LỜI CÁM ƠN Sau thời gian học tập nghiên cứu trường, hồn thành đề tài tốt nghiệp cao học Để có thành này, tơi nhận nhiều hỗ trợ giúp đỡ tận tình từ thầy cơ, gia đình bạn bè Tơi xin chân thành cảm ơn quý Thầy cô khoa Quản Lý Khoa Học - Đào Tạo Sau Đại Học, quý Thầy cô khoa Cơ - Điện - Điện Tử Trường Đại Học Kỹ Thuật Cơng Nghệ TP.HCM tận tình giúp đỡ, hỗ trợ tơi suốt q trình thực luận văn Với lịng tri ân sâu sắc, tơi muốn nói lời cám ơn đến Thầy TS Nguyễn Thanh Phương, người nhiệt tình hướng dẫn bảo cho suốt thời gian thực nghiên cứu Tp Hồ Chí Minh, tháng 02 năm 2014 Người thực luận văn Phạm Quốc Thiện iii TÓM TẮT LUẬN VĂN Trong báo này, điều khiển phi tuyến dựa phương pháp Lyapunov đề xuất áp dụng cho xe tự hành Đầu tiên quỹ đạo − (G ) với đa thức bậc thiết kế Thứ hai, dựa vào vị trí ước lượng xe tự hành, điều khiển làm cho xe tự hành bám theo quỹ đạo G3 di chuyển với vận vận tốc không đổi Sự ổn định hệ thống chứng minh phương pháp Lyapunov Mơ kết thực nghiệm trình bày để chứng minh tính hiệu điều khiển đề xuất iv ABSTRACT In this paper, a nonlinear controller base on Lyapunov method is proposed and applied for wheel mobile robot (WMR) First, trajectory − (G3) with 7th order polinom is designed for WMR Then, based on the estimated position of the WMR, this controller makes WMR follow trajectory G3 which is moving with desired constant velocity The stability of system is proved by the Lyapunov stability theory The simulations and experimental results are shown to prove the effectiveness of the proposed controller v MỤC LỤC Tên đề mục Trang Lời cam đoan i Lời cảm ơn ii Tóm tắt luận văn .iii Abstract iv Mục lục v Danh mục từ viết tắt vii Danh mục bảng biểu viii Danh mục sơ đồ, hình ảnh ix Chương 1: Mở đầu 1.1 Đặt vấn đề 1.2 Tính cấp thiết đề tài 1.3 Mục tiêu đề tài 1.4 Nội dung nghiên cứu 1.5 Phương pháp luận 1.6 Phương pháp nghiên cứu 1.7 Nội dung luận văn Chương 2: Tổng quan AGV 2.1 Sơ lược trình phát triển AGV 2.2 Phân loại AGV .5 2.2.1 Tàu không người lái 2.2.2 Xe nâng pallet 2.2.3 Xe chở hàng 2.3 Tóm tắt cơng trình nghiên cứu 2.4 Nhận xét hướng tiếp cận 32 Chương 3: Mơ hình tốn học AGV 33 3.1 Cấu trúc AGV 33 vi 3.2 Xây dựng phương trình động học 34 3.3 Xây dựng quỹ đạo đường cho AGV 36 Chương 4: Thiết kế điều khiển hệ thống đo lường 39 4.1 Cơ sở lý thuyết 39 4.1.1 Giới thiệu phương pháp Lyapunov 39 4.1.2 Điểm cân hệ phi tuyến 39 4.1.3 Ổn định điểm cân .41 4.1.3.1 Định nghĩa 41 4.1.3.2 Ổn định ổn định tiệm cận Lyapunov 41 4.1.4 Phương pháp tuyến tính hóa Lyapunov 42 4.1.5 Phương pháp trực tiếp Lyapunov 44 4.1.5.1 Định lý ổn định Lyapunov .44 4.1.5.2 Định lý không ổn định Lyapunov 45 4.2 Thiết kế điều khiển 47 4.3 Hệ thống đo lường 48 Chương 5: Thiết kế thi công AGV mô 51 5.1 Thiết kế mơ hình 51 5.2 Thiết kế mạch điện 53 5.3 5.4 5.2.1 Phương án thiết kế 53 5.2.2 Sơ đồ nguyên lý 54 5.2.3 Thiết bị sử dụng 57 Kết mô thực nghiệm 60 5.3.1 Kết mô trường hợp 61 5.3.2 Kết mô trường hợp 63 Kết luận hướng phát triển 65 TÀI LIỆU THAM KHẢO PHỤ LỤC vii DANH MỤC CÁC TỪ VIẾT TẮT AGV Automated Guided Vehicles GPS Global Positioning System WMR Wheel Mobile Robot RF Radio Frequency FL Feedback Linearization SMC Sliding Mode Control FSMC Fuzzy Sliding Mode Control WP Way Point cm centimeter s second DC Direct Current RPM Revolutions Per Minute viii DANH MỤC CÁC BẢNG BIỂU Trang Bảng 2.1 Luật điều khiển mờ 19 Bảng 2.2 Giá trị thông số AGV 27 Bảng 2.3 Giá trị khởi tạo ban đầu 27 Bảng 5.1 Thông số mô 60 ix DANH MỤC CÁC LƯU ĐỒ, HÌNH ẢNH Trang Hình 2.1 AGV dạng tàu khơng người lái Hình 2.2 AGV dạng xe nâng pallet Hình 2.3 AGV dạng xe chở hàng Hình 2.4 Phương pháp tính tốn vị trí Hình 2.5 Mơi trường thực nghiệm 10 Hình 2.6 Kết sử dụng lọc Kalman 10 Hình 2.7 Dữ liệu vị trí 11 Hình 2.8 Sơ đồ khối vòng điều khiển robot di động Hình 2.9 bám theo quỹ đạo G3 12 Quỹ đạo mong muốn quỹ đạo thực robot 12 Hình 2.10 Robot di động hai bánh 13 Hình 2.11 Vị trí thật mục tiêu robot 13 Hình 2.12 Đáp ứng vận tốc bánh trái phải 14 Hình 2.13 Sai số quỹ đạo , , 14 Hình 2.14 Sơ đồ khối vịng điều khiển mobile robot Hình 2.15 Điều khiển vịng kín với = 0.6 , 15 =2 để mơ robot Quỹ đạo robot tín hiệu ngõ với nhiễu tín hiệu ngõ vào Hình 2.16 Điều khiển vịng kín với = 0.6 , 16 =2 để mơ robot Quỹ đạo robot tín hiệu ngõ với nhiễu tín hiệu ngõ vào 17 Hình 2.17 Sơ đồ khối SMC 18 Hình 2.18 Sơ đồ khối FSMC 19 Hình 2.19 Hàm membership ngõ vào-ngõ , ̇ , 20 Hình 2.20 WMR bám theo quỹ đạo tham chiếu 20 x Hình 2.21 Sai số ( − ) 21 Sai số ( − ) 21 Hình 2.22 Hình 2.23 Tín hiệu điều khiển torque cho bánh phải Hình 2.24 Sai số góc định hướng Hình 2.25 − Sai số ( Sai số ( 23 − ): (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC Hình 2.27 22 WMR bám theo quỹ đạo tham chiếu: (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC Hình 2.26 22 23 − ): (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC Hình 2.28 Sai số góc định hướng 24 − : (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC 24 Hình 2.29 Tín hiệu điều khiển torque cho bánh phải: (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC Hình 2.30 Lưu đồ giải thuật điều khiển tuyến tính hồi tiếp Hình 2.31 25 26 Quỹ đạo mong muốn AGV có dạng đường thẳng ( y = x ) 26 Hình 2.32 Quỹ đạo AGV thời gian ban đầu 28 Hình 2.33 Sai lệch vị trí tồn thời gian 28 Hình 2.34 Vận tốc tuyến tính AGV tồn thời gian 29 Hình 2.35 Vận tốc góc bánh phải bánh trái quỹ đạo đường thẳng 29 Hình 2.36 Véc tơ điều khiển đầu vào 30 Hình 2.37 Véc tơ điều khiển đầu vào u 30 Hình 2.38 Véc tơ điều khiển đầu vào 31 Hình 3.1 Mơ hình động học WMR 33 Hình 3.2 Khái niệm AGV bám theo quỹ đạo tham chiếu 36 Hình 3.3 Quỹ đạo G3 qua hai điểm A B 38 Hình 4.1 Sơ đồ khối vòng điều khiển AGV bám quỹ đạo G3 49 %record(s,'off'); fclose(s); delete(s); clear s; end; close(WMR); % - Executes during object creation, after setting all properties function Trajectory_CreateFcn(hObject, eventdata, handles) % hObject handle to Trajectory (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: place code in OpeningFcn to populate Trajectory function editstr1_Callback(hObject, eventdata, handles) % hObject handle to editstr1 (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 editstr1 as text % str2double(get(hObject,'String')) returns contents of editstr1 as a double % - Executes during object creation, after setting all properties function editstr1_CreateFcn(hObject, eventdata, handles) % hObject handle to editstr1 (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 editstr2_Callback(hObject, eventdata, handles) % hObject handle to editstr2 (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 editstr2 as text % str2double(get(hObject,'String')) returns contents of editstr2 as a double % - Executes during object creation, after setting all properties function editstr2_CreateFcn(hObject, eventdata, handles) % hObject handle to editstr2 (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 selection change in Com function Com_Callback(hObject, eventdata, handles) % hObject handle to Com (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 Com contents as cell array % contents{get(hObject,'Value')} returns selected item from Com % - Executes during object creation, after setting all properties function Com_CreateFcn(hObject, eventdata, handles) % hObject handle to Com (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 function editstr3_Callback(hObject, eventdata, handles) % hObject handle to editstr3 (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 editstr3 as text % str2double(get(hObject,'String')) returns contents of editstr3 as a double % - Executes during object creation, after setting all properties function editstr3_CreateFcn(hObject, eventdata, handles) % hObject handle to editstr3 (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 editstr4_Callback(hObject, eventdata, handles) % hObject handle to editstr4 (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 editstr4 as text % str2double(get(hObject,'String')) returns contents of editstr4 as a double % - Executes during object creation, after setting all properties function editstr4_CreateFcn(hObject, eventdata, handles) % hObject handle to editstr4 (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 send function send_Callback(hObject, eventdata, handles) % hObject handle to send (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = get(handles.editstr1,'String'); send_data = str2num(send_data); fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in pushbutton10 function pushbutton10_Callback(hObject, eventdata, handles) % hObject handle to pushbutton10 (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 forward function forward_Callback(hObject, eventdata, handles) % hObject handle to forward (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 105; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles) % - Executes on button press in right function right_Callback(hObject, eventdata, handles) % hObject handle to right (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 107; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in left function left_Callback(hObject, eventdata, handles) % hObject handle to left (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 106; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in reserve function reserve_Callback(hObject, eventdata, handles) % hObject handle to reserve (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 108; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in Up function Up_Callback(hObject, eventdata, handles) % hObject handle to Up (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 103; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in Down function Down_Callback(hObject, eventdata, handles) % hObject handle to Down (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 104; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in Run function Run_Callback(hObject, eventdata, handles) % hObject handle to Run (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 100; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in Line function Line_Callback(hObject, eventdata, handles) % hObject handle to Line (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 102; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); % - Executes on button press in Read function Read_Callback(hObject, eventdata, handles) % hObject handle to Read (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) IsConnect = handles.status_com; s = handles.com; if (IsConnect == 1) send_data = 101; fwrite(s,send_data); end handles.com = s; guidata(hObject,handles); Quỹ đạo đường định sẵn cho AGV Chương trình Matlab: clear all; close all; clc; xa = 0; ya = 0; xb = 1.4; yb = 1.4; theta_a = 0; theta_b = 0; pre_theta_r = 0; n = sqrt((xa - xb)^2 + (ya - yb)^2); syms s ; for i = 0:10 s = i*0.1; % xac dinh he so px a0 = xa; a1 = n*cos(theta_a); y1a = n*cos(theta_b); a2 = 0; a3 = 0; a4 = 35*(xb - xa) - 20*a1 - 10*a2 - 4*a3 - 15*y1a; a5 = -84*(xb - xa) + 45*a1 + 20*a2 + 6*a3 + 39*y1a; a6 = 70*(xb - xa) - 36*a1 - 15*a2 - 4*a3 - 34*y1a; a7 = -20*(xb - xa) + 10*a1 + 4*a2 + a3 + 10*y1a; px = a0 + a1*s + a2*s^2 + a3*s^3 + a4*s^4 + a5*s^5 + a6*s^6 + a7*s^7; % xac dinh he so py b0 = ya; b1 = n*sin(theta_a); y1b = n*sin(theta_b); b2 = 0; b3 = 0; b4 = 35*(yb - ya) - 20*b1 - 10*b2 - 4*b3 - 15*y1b; b5 = -84*(yb - ya) + 45*b1 + 20*b2 + 6*b3 + 39*y1b; b6 = 70*(yb - ya) - 36*b1 - 15*b2 - 4*b3 - 34*y1b; b7 = -20*(yb - ya) + 10*b1 + 4*b2 + b3 + 10*y1b; py = b0 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6 + b7*s^7; px_out(i+1) = px; py_out(i+1) = py; end save D:\data1: px_out py_out % Ve quy dao duong cong plot(px_out,py_out,'b', 'LineWidth',2); title('Trajectory planning'); xlabel('Px'); ylabel('Py'); axis([0 1.5 1.5]); grid on; Giao tiếp I2C RS232 cho vi điều khiển Firmware sử dụng CCS - Cấu hình firmware: Khai báo chip: chọn file có thu viện CCS, VD : #include Khai báo cấu hình phần cứng: sử dụng từ khóa #fuse bao gồm tính : LP, XT, HS, RC,PLL : Chọn kiểu dao dộng WDT, NOWDT : Kiểm soát Watch Dog Timer PROTECT, NOPROTECT : Kiểm soát chép firmware LVP,NOLVP: Kiểm sốt lập trình nguồn thấp Khai báo cấu hình chức nang sử dụng từ khóa #use bao gồm tính năng: delay(clock=24000000) : Khai báo tốc dộ thạch anh i2c(master, sda=PIN_B0, scl=PIN_B1, FAST): Khai báo cấu hình I2C rs232(baud=19200, xmit=PIN_C6, rcv=PIN_C7): Khai báo cấu hình cổng COM Khai báo dịch vụ ngắt, sử dụng từ khóa: INT_SSP : Ngắt nối tiếp ghi dịch vừa thực xong bit s INT_EXT : Ngắt ngồi INT_TIMER1: Ngắt Timer INT_EEPROM: Ngắt có kiện truy xuất EEPROM Chương trình main() Master: while (TRUE) { Nội dung chương trình } Ghi liệu tới Slave: i2c_start(); // Master tạo tín hiệu Start i2c_write(0xa0); // Gửi dịa bít huớng R/W = (Mode write) dến Slave i2c_write(data); // Gửi liệu byte thứ i2c_write(data1); // Gửi liệu byte thứ hai ………………… i2c_write(datan); // Gửi liệu byte thứ n (n