Điều khiển thích nghi robot di động dạng xe bám quỹ đạo

Đại Học Quốc Gia Tp Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA LÊ ANH TUẤN ĐIỀU KHIỂN THÍCH NGHI ROBOT DI ĐỘNG DẠNG XE BÁM QUỸ ĐẠO Chuyên ngành: Tự động hóa Mã số ngành: 60.52.60 LUẬN VĂN THẠC SĨ Tp Hồ Chí Minh, tháng 11 năm 2009 Đại Học Quốc Gia Tp Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA LÊ ANH TUẤN ĐIỀU KHIỂN THÍCH NGHI ROBOT DI ĐỘNG DẠNG XE BÁM QUỸ ĐẠO Chuyên ngành: Tự động hóa Mã số ngành: 60.52.60 LUẬN VĂN THẠC SĨ Tp Hồ Chí Minh, tháng 11 năm 2009 i 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: TS Huỳnh Thái Hoàng Cán chấm nhận xét 1: Cán chấm nhận xét 2: Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày tháng năm 2009 ii  TRƯỜNG ĐH BÁCH KHOA TP HCM PHÒNG ĐÀO TẠO SĐH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc Tp HCM, ngày tháng năm 2009 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: Lê Anh Tuấn Phái: Nam Ngày, tháng, năm sinh: 09/03/1983 Nơi sinh: Quảng Nam Chuyên ngành: Tự động hóa MSHV: 01507324 I- TÊN ĐỀ TÀI: Điều khiển thích nghi robot di động dạng xe bám quĩ đạo II-NHIỆM VỤ VÀ NỘI DUNG: II.1 Tìm hiểu lý thuyết sở để thực luận văn II.2 Xây dựng mơ hình tốn thuật tốn điều khiển thích nghi áp dụng điều khiển robot di động dạng xe bám theo quỹ đạo cho trước ảnh hưởng yếu tố bất định II.3 Viết chương trình mơ Matlab để kiểm chứng lý thuyết II.4 Thực nghiệm Soccer robot III- NGÀY GIAO NHIỆM VỤ: 02/02/2009 IV- NGÀY HOÀN THÀNH NHIỆM VỤ: 30/11/2009 V- CÁN BỘ HƯỚNG DẪN: TS Huỳnh Thái Hoàng CÁN BỘ HƯỚNG DẪN CN BỘ MÔN QL CHUYÊN NGÀNH 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… TRƯỞNG PHÒNG ĐT – SĐH TRƯỞNG KHOA QL NGÀNH iii LỜI CẢM ƠN Qua thời gian thực luận văn, học viên học hỏi nhiều kiến thức bổ ích Đạt điều nhờ vào giảng dạy truyền đạt kiến thức quý thầy cô trường Đại học Bách khoa, tạo tảng cho cho học viên tiếp thu giải khó khăn trình làm luận văn Đầu tiên học viên thực luận văn xin chân thành cảm ơn ban giám hiệu, phòng đào tạo sau đại học, quý thầy cô trường, đặc biệt quý thầy cô khoa Điện – Điện tử, môn Điều khiển Tự động, quan tâm tạo điều kiện thuận lợi cho học viên hoàn thành luận văn Học viên xin bày tỏ lòng biết ơn sâu sắc đến thầy Huỳnh Thái Hồng Với giúp đỡ tận tình, ln quan tâm, động viên theo sát trình thực luận văn, thầy gợi ý cho học viên thực luận văn có ý tưởng để giải vấn đề khó khăn gặp phải thực luận văn, hoàn thành tốt nhiệm vụ đề Học viên xin bày tỏ lòng biết ơn đến ban quản lý phịng thí nghiệm trọng điểm quốc gia Điều khiển số kỹ thuật hệ thống tạo điều kiện cho học viên tiếp xúc nghiên cứu hệ thống Soccer robot để tiến hành phần thực nghiệm luận văn Bên cạnh giúp đỡ tận tình thầy hướng dẫn, cịn có giúp đỡ người thân, bạn học người xung quanh, học viên thực luận văn xin gửi lời cảm ơn đến tất người quan tâm, giúp đỡ để học viên đạt kết hôm Tp Hồ Chí Minh, tháng 11 năm 2009 Lê Anh Tuấn iv TÓM TẮT Hiện nay, theo đà phát triển khoa học kỹ thuật, robot đề tài mẻ hấp dẫn để nghiên cứu phát triển mà cụ thể robot di động dạng xe Nghiên cứu robot loại gia tăng thu hút nhiều ý năm gần chúng sử dụng ngày tăng phạm vi ứng dụng rộng Các robot di động dạng xe áp dụng hầu hết lĩnh vực khác từ khơng gian, đại dương, thăm dị núi lửa, di chuyển vật liệu nhà máy, ứng dụng để phục vụ cho nhu cầu thiết yếu người sống Cũng đề tài gần gũi với thực tiễn mà nảy sinh nhiều vấn đề phức tạp cần giải Một vấn đề đầy thách thức toán điều khiển robot di động dạng xe bám quỹ đạo mong muốn, việc xem xét cách để điều khiển robot bám quỹ đạo cho đạt hiệu ứng dụng cụ thể Có nhiều phương pháp điều khiển robot khác điều khiển thích nghi [1], [2], [3], điều khiển trượt [4], điều khiển dùng giải thuật di truyền (GA) hay lập trình Gen _ GP (Genetic Programming) [5], điều khiển mờ hay mạng thần kinh [6], điều khiển dự đoán dựa vào mơ hình [7],… Tùy theo phương pháp mà cho kết điều khiển bám khác Trong phương pháp trên, nói điều khiển thích nghi phương pháp thích hợp để điều khiển robot bám quỹ đạo trình robot di chuyển có nhiều loại nhiễu tác động vào, phương pháp thích nghi với thay đổi mơi trường chất lượng điều khiển không đổi hay thay đổi phạm vi chấp nhận Ví dụ như, robot phải di chuyển theo quỹ đạo cho trước, chí có ma sát lớn với mặt sàn hay chịu tải trọng trình di chuyển Khi có tác động từ bên ngồi lên robot, thông số điều khiển thay đổi để đảm bảo chất lượng điều khiển tốt Trong luận văn này, phương pháp điều khiển thích nghi sử dụng để điều khiển loại robot di động dạng xe bám quỹ đạo ảnh hưởng yếu tố bất định tải trọng thay đổi, ma sát bề mặt hay nhiễu ngồi Bộ điều khiển thích nghi thiết kế dựa vào lý thuyết ổn định Lyapunov Sau hồn thành mơ phần mềm Matlab, phương pháp xem xét liệu kiểm chứng Soccer robot thực phịng thí nghiệm v ABSTRACT At present, together with the development of science and technology, robot is always a novel and attractive topic to study and develop Particularly, researchers have been increasing their attention to wheeled mobile robots (WMRs) for some recent years because of their usefulness in a large range of applications in different fields such as discovering space, researching on ocean, exploring volcanoes, transferring materials in factories, or serving disabled people, etc One of the challenging problems is trajectory tracking control of wheeled mobile robots Many methods have been proposed to solve this problem, namely adaptive control [1], [2], [3], sliding mode control [4], control based on genetic algorithm or genetic programming [5], fuzzy control or neural network [6], model-based predictive control [7], and so on The performance of trajectory tracking control for wheeled mobile robots is different depended on every special method of control Many researches showed that adaptive control is so potential a method to drive WMRs following the desired trajectory There will have uncertainties affecting the robot while it is moving in environment and this method has the ability to adapt to every change of noises to give the good control performance For example, robot can track the trajectory well under factors of uncertainties such as unexpected changed load, surface friction or unknown external disturbances When robot is affected by noises, the parameters of the adaptive controller can be changed arbitrarily to compensate the control errors appearing due to these uncertainties In this thesis, method of adaptive control is used to control the wheeled mobile robot tracking trajectory and the proposed controller is designed thanks to Lyapunov stability theory After finishing the simulation with Matlab, a Soccer robot in the laboratory will be considered whether it can be used to verify the adaptive control algorithm Keywords: Wheeled mobile robot; Adaptive control; Lyapunov stability; Trajectory tracking vi MỤC LỤC Đề mục Trang Trang tựa i Trang kế ii Nhiệm vụ luận văn thạc sĩ iii Lời cảm ơn iv Tóm tắt v Abstract vi Mục lục vii Liệt kê bảng ix Liệt kê hình x Chương I I.1 I.2 I.3 I.4 I.5 Lịch sử phát triển robot giới thiệu robot di động dạng xe (WMRs) Tổng quan toán điều khiển robot di động dạng xe bám quỹ đạo .7 Mục tiêu luận văn 10 Phương pháp nghiên cứu 11 Sơ lược nội dung luận văn 11 Chương II II.1 II.2 II.3 II.4 II.5 Xây dựng thuật tốn điều khiển thích nghi robot di động dạng xe Cơ sở lý thuyết 33 Hệ thống lái WMRs 53 Robot tham chiếu 53 Điều khiển động học 54 Thiết kế điều khiển thích nghi động lực học .59 Điều khiển bám quỹ đạo tham chiếu 66 Hệ thống điều khiển động DC bánh lái 67 Chương IV IV.1 IV.2 IV.3 IV.4 Xây dựng mơ hình tốn cho robot di động dạng xe Cơ sở lý thuyết 13 Phương trình động học 22 Phương trình động lực học 26 Mô hình biến trạng thái 30 Cấu trúc tính chất phương trình robot 30 Chương III III.1 III.2 III.3 III.4 III.5 III.6 III.7 Tổng quan Mô với Matlab Mô phương trình động lực học WMRs 73 Mô quĩ đạo robot tham chiếu 79 Mơ thuật tốn điều khiển thích nghi 81 Mô thuật tốn điều khiển thích nghi sau thêm khâu PID 87 vii Chương V V.1 V.2 V.3 V.4 V.5 V.6 Thực nghiệm Soccer robot Đặt vấn đề 89 Hệ thống Soccer robot 90 Các tham số hệ thống Soccer robot 91 Giải thuật lập trình 91 Tìm vị trí đối tượng màu ảnh hai chiều .93 Giao diện điều khiển 94 Chương VI Kết luận VI.1 Đánh giá kết luận văn .97 VI.2 Hướng phát triển .97 Tài liệu tham khảo A Phụ lục A: Chương trình Matlab E Phụ lục B: Chương trình C++ K Lý lịch trích ngang viii LIỆT KÊ BẢNG Bảng IV.1 IV.2 IV.3 IV.4 Trang Khai báo giá trị thông số WMRs 73 Kết mô ứng với thông số moment khác hai bánh lái 74 Các thông số điều khiển 82 Kết điều khiển bám quỹ đạo thêm điều khiển PID 88 ix % set up the Parameters DC motors of driving wheels % %Armature resistance (ohm) R_a=2; %Armature inductance (H) L_a=0.5; %Armature moment constant K_ma=0.0053; %Frictional coefficient on the shaft of DC motor B=4e-5; %Back EMF constant K_b=0.003; % % DC motor transfer % ts1=[J_w*K_ma K_ma*B]; ms1=[L_a*J_w (R_a*J_w+B*L_a) (R_a*B+K_b*K_ma)]; Gm=tf(ts1,ms1) figure(1); bode(Gm); grid on; Title('Bode diagram of DC motor transfer'); jun=0.707; T=0.001; Wn=1/T; ts2=1; ms2=[T^2 2*jun*T 1]; G_to=tf(ts2,ms2) figure(2); bode(G_to); grid on; Title('Bode diagram of desired transfer'); % % Chosing the suitable Parameters of PID controller for DC motors of driving wheels % K=100; %K=0.5685; T1=50; T2=0.00001; T3=0.001; ts3=K*conv([T1 1],[T2 1]); ms3=conv([1 0],[T3 1]); F G_PID=tf(ts3,ms3) figure(3); bode(G_PID); grid on; Title('Bode diagram of PID controller'); set_param('noise_three_wheel_mobibe_robot_control29/Subsystem1/PID controller1', 'Numerator', 'ts3', 'Denominator', 'ms3'); set_param('noise_three_wheel_mobibe_robot_control29/Subsystem1/PID controller2', 'Numerator', 'ts3', 'Denominator', 'ms3'); Gh=G_PID*Gm; figure(4); bode(Gh); grid on; Title('Bode diagram of open system'); % sim('noise_three_wheel_mobibe_robot_control29',47.8); Gk=minreal(Gh/(1+Gh)) %************** %end setting up ******************************************* ** -**Tạo quỹ đạo mong muốn** -** ******************************************* clc; clear all; t_si=47.8; t=0:1e-3:t_si; %************************************** vrd=0.1; v_r=vrd*ones(size(t)); for t2=1:4000 ti1=0.001; ti2=4; v_r(t2)=vrd*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=2001:4000 ti1=2; ti2=4; v_r(t2)=vrd; end %************************************** w_r=0*ones(size(t)); for t2=3000:6000 ti1=3; G ti2=6; w_r(t2)=computing_w_m(ti1,ti2,pi/2)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=8000:10000 ti1=8; ti2=10; w_r(t2)= computing_w_m(ti1,ti2,-pi/2)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=10000:12000 ti1=10; ti2=12; w_r(t2)= computing_w_m(ti1,ti2,-pi/2)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=12000:15000 ti1=12; ti2=15; w_r(t2)= computing_w_m(ti1,ti2,pi/2)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=15000:18000 ti1=15; ti2=18; w_r(t2)= computing_w_m(ti1,ti2,pi/2)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=24000:26000 ti1=24; ti2=26; w_r(t2)= computing_w_m(ti1,ti2,pi/2)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=28000:30000 ti1=28; ti2=30; w_r(t2)= computing_w_m(ti1,ti2,pi/4)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=32000:34000 ti1=32; ti2=34; w_r(t2)= computing_w_m(ti1,ti2,-pi/4)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=36000:38000 ti1=36; ti2=38; w_r(t2)= computing_w_m(ti1,ti2,pi/4)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=39000:42000 ti1=39; ti2=42; w_r(t2)= computing_w_m(ti1,ti2,45*pi/180)*sin((pi/(ti2-ti1))*(t2)/1000); end for t2=42000:45000 ti1=42; H ti2=45; w_r(t2)= computing_w_m(ti1,ti2,57*pi/180)*sin((pi/(ti2-ti1))*(t2)/1000); end sig_v=[t;v_r]; sig_w=[t;w_r]; save v_r.mat sig_v save w_r.mat sig_w -****************************** ** -**Tính ωm** -** ****************************** function w_m=computing_w_m(t1,t2,phi) w_m=phi/(((t1-t2)/pi)*(cos(pi*t2/(t2-t1))-cos(pi*t1/(t2-t1)))); end ******************************************* ** -**Vẽ đồ thị** -** ******************************************* clc; % close all; N=47800; for t=1:N+1 xn(t)=x_o(t); yn(t)=y_o(t); getto_r(t)=to_r(t); getto_l(t)=to_l(t); getto_r1(t)=to_r1(t); getto_l1(t)=to_l1(t); en1_1(t)=e_1(t); en2_2(t)=e_2(t); en3_3(t)=e_3(t); end xn_o=xn'; yn_o=yn'; ton_r=getto_r'; ton_l=getto_l'; ton_r1=getto_r1'; ton_l1=getto_l1'; en_1=en1_1'; en_2=en2_2'; en_3=en3_3'; a_width=1; s_width=1.5; f_size=8; f_wt = 'normal'; grd='off'; I hdl=figure(5); set(hdl,'Color', [1 1],'Position', [300 200 394 290]); % subplot(2,1,1); hdl=plot(x_r,y_r,'k'); set(hdl,'LineWidth',s_width); hdl=gca; set(hdl,'XLim',[0 1.5], 'YLim',[0 1.3], 'XTick',[0 0.2 0.4 0.6 0.8 1.2 1.4], 'XTickLabel',[0 0.2 0.4 0.6 0.8 1.2 1.4], 'YTick',[0 0.2 0.4 0.6 0.8 1.2], 'XGrid',grd, 'YGrid',grd, 'LineWidth',a_width, 'FontSize', f_size, 'FontWeight',f_wt); Title('XY plots of reference and actuality robots'); hold on hdl=plot(xn_o,yn_o,'b:'); set(hdl,'LineWidth',s_width); L=legend('Reference trajectory', 'Actuality trajectory',2); set(L, 'LineWidth',1.5,'LineStyleOrder',' '); Xlabel('X Axis'); Ylabel('Y Axis'); figure(6); subplot(2,1,1); plot(ton_r,'b'); hold on plot(ton_r1,'r '); Grid on; Title('Moment of right wheel'); h=legend('to_rref','to_rdes',4); subplot(2,1,2); hdl=plot(ton_l,'b'); hold on plot(ton_l1,'r '); Grid on; Title('Moment of left wheel'); h=legend('to_lref','to_ldes',4); figure(7); subplot(3,1,1); plot(en_1,'b'); Grid on; J Title('Position Error between x_0 & x _r'); h=legend('e1',4); subplot(3,1,2); plot(en_2,'b'); Grid on; Title('Position Error between y_0 & y _r'); h=legend('e2',4); subplot(3,1,3); plot(en_3,'b'); Grid on; Title('Position Error between phi & phi_r'); h=legend('e3',4); Phụ lục B: Chương trình C++ ******************************* /* Trajectory_Reference_Robot.cpp */ #include using std::cerr; using std::endl; #include "Trajectory_Reference_Robot_C.h" int main() { cerr