Đang tải... (xem toàn văn)
Sử dụng matlab để mô phỏng việc fusion tín hiệu cảm biến GPS và odometry sau đó điều khiển robot tự hành bằng lý thuyết ổn định lyapunovLuận văn có 5 chương và phần phụ lục code matlab kèm theo:Chương 1: Mở đầuChương 2: Tổng quanChương 3: Cơ sở lý thuyếtChương 4: Thiết kế giải thuậtChương 5: Kết luận và hướng phát triển.Phụ lục chương trình matlab
ĐẠI HỌC QUỐC GIA TP HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA HỌ VÀ TÊN HUỲNH THANH TUẤN TÊN ĐỀ TÀI LUẬN VĂN THẠC SĨ NGHIÊN CỨU TỰ HÀNH ROBOT DỰA TRÊN PHƯƠNG PHÁP KẾT HỢP NHIỀU CẢM BIẾN (Research on autonomous robots based on the multiple sensor fusion methods) Chuyên ngành: CƠ ĐIỆN TỬ Mã số: 8520114 Số điện thoại: 0936641141 Email: 1870245@hcmut.edu.vn LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng 12 năm 2021 I ĐẠI HỌC QUỐC GIA TP.HCM CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM TRƯỜNG ĐẠI HỌC BÁCH KHOA Độc lập - Tự - Hạnh phúc NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: HUỲNH THANH TUẤN Ngày tháng năm sinh: 11/05/1992 Chuyên ngành: Kỹ thuật Cơ điện tử MSHV: 1870245 Nơi sinh: Tỉnh Bình Định Mã số: 8520114 I TÊN ĐỀ TÀI: Nghiên cứu tự hành robot dựa phương pháp kết hợp nhiều cảm biến (Research on autonomous robots based on the multiple sensor fusion methods) II NHIỆM VỤ VÀ NỘI DUNG: - Thiết kế giải thuật kết hợp nhiều cảm biến để định vị robot tự hành - Mô giải thuật kết hợp nhiều cảm biến để định vị robot tự hành - Thiết kế điều khiển robot bám quỹ đạo - Mô điều khiển robot bám quỹ đạo dựa kết hợp nhiều cảm biến III NGÀY GIAO NHIỆM VỤ: 06/09/2021 V NGÀY HOÀN THÀNH NHIỆM VỤ: 28/12/2021 IV CÁN BỘ HƯỚNG DẪN: PGS.TS NGUYỄN TẤN TIẾN Tp HCM, ngày … tháng … năm 20… CÁN BỘ HƯỚNG DẪN (Họ tên chữ ký) CHỦ NHIỆM BỘ MÔN ĐÀO TẠO (Họ tên chữ ký) TRƯỞNG KHOA (Họ tên chữ ký) II LỜI CẢM ƠN Trước tiên xin phép gửi lời cảm ơn chân thành đến PGS TS NGUYỄN TẤN TIẾN giảng viên hướng dẫn trực tiếp Cảm ơn thầy hỗ trợ gặp vấn đề có câu hỏi vấn đề nghiên cứu Thầy ln cho phép tơi tự bày tỏ quan điểm đồng thời đưa nhận xét, góp ý, dẫn dắt tơi hướng suốt thời gian nghiên cứu, thực đề tài luận văn Tôi xin cảm ơn thầy khoa Cơ Khí - Trường Đại học Bách Khoa - Đại học quốc gia Hồ Chí Minh truyền đạt cho kiến thức chuyên ngành suốt thời gian học tập để tơi có tảng kiến thức hỗ trợ lớn cho tơi q trình làm luận văn Tơi muốn bày tỏ biết ơn đến TS DƯƠNG VĂN TÚ, anh có nhiều nhận xét góp ý có giá trị giúp tơi hồn thiện luận văn Cuối cùng, xin gửi lời cảm ơn đến gia đình bạn bè ln hỗ trợ tơi khuyến khích liên tục suốt năm học tập qua trình nghiên cứu viết luận văn Kết khơng thể có khơng có họ Xin chân thành cảm ơn! III TÓM TẮT LUẬN VĂN Nhiệm vụ luận văn tập trung vào việc nghiên cứu phương pháp kết hợp liệu nhiều loại cảm biến để xác định vị trí góc robot tự hành q trình di chuyển Phương pháp kết hợp liệu cảm biến áp dụng luận văn sử dụng lọc Kalman mở rộng để kết hợp hai loại cảm biến có ưu điểm bổ sung cho toán định vị robot cảm biến định vị cục định vị toàn cục: Odometry GPS, UWB Luận văn cịn trình bày thiết kế điều khiển robot bám theo quỹ đạo dựa kết hợp nhiều cảm biến Luận văn hoàn thành yêu cầu thiết kế giải thuật kết hợp liệu cảm biến thiết kế điều khiển để điều khiển robot bám quỹ đạo Trên sở mơ hình chế tạo tiến hành thực nghiệm khả bám quỹ đạo kết thực nghiệm đáp ứng yêu cầu đề IV ABSTRACT The task of the thesis focuses on studying the method of combining data of many types of sensors to determine the position and pose of the self-propelled robot during the movement The method of combining sensor data applied in the thesis is to use an extended Kalman filter to combine two types of sensors with complementary advantages in the robot positioning problem, local localization sensors and global localization sensors Sensor used in the thesis Odometry and GPS, UWB The thesis also presents the design of the controller to control the robot to follow the trajectory based on the combination of many sensors The thesis has completed the requirements of designing an algorithm that combines sensor data and designing a controller to control the orbiting robot On the basis of the manufactured model, conduct orbital tracking and experimental results meet the requirements of the topic V LỜI CAM ĐOAN Tôi xin cam đoan tất nội dung luận văn không chép cơng trình nghiên cứu cá nhân hay tổ chức khác Tơi thực nghiêm túc việc trích dẫn cơng trình báo, sách, tham luận nguồn khác có sử dụng luận văn Tác giả luận văn Huỳnh Thanh Tuấn VI MỤC LỤC: NHIỆM VỤ LUẬN VĂN THẠC SĨ II LỜI CẢM ƠN III TÓM TẮT LUẬN VĂN IV ABSTRACT V LỜI CAM ĐOAN VI MỤC LỤC: VII MỤC LỤC HÌNH ẢNH: IX Chương 1.1 MỞ ĐẦU: Nhu cầu thực tiễn đề tài: 1.1.1 Đặt vấn đề: 1.1.2 Những ưu điểm hạn chế việc kết hợp cảm biến: 1.2 Mục tiêu đề tài: Chương 2.1 TỔNG QUAN: Tổng quan tổng hợp liệu: 2.1.1 Phân loại phương pháp kết hợp liệu: 2.1.2 Các kỹ thuật kết hợp liệu: 15 2.1.3 Các phương pháp ước tính trạng thái: 20 2.2 Phương án thực luận văn: 24 Chương 3.1 CƠ SỞ LÝ THUYẾT: 26 Bộ lọc Kalman: 26 3.1.1 Tổng quan tính tốn: 26 3.1.2 Mơ hình lọc Kalman tiêu chuẩn: 27 Chương THIẾT KẾ GIẢI THUẬT KẾT HỢP CẢM BIẾN VÀ BỘ ĐIỀU KHIỂN: 31 4.1 Thiết kế giải thuật kết hợp cảm biến lọc Kalman mở rộng (EKF): 31 4.1.1 Định vị robot EKF: 31 4.1.2 Kết hợp cảm biến định vị robot EKF: 36 4.1.3 Mô kết hợp cảm biến định vị robot EKF với tín hiệu giả lập: 41 4.1.4 Kiểm chứng giải thuật kết hợp cảm biến với liệu cảm biến thực: 44 4.2 Thiết kế điều khiển: 52 4.2.1 Mơ hình động học robot: 52 4.2.2 Thiết kế điều khiển: 53 4.3 Kết mô điều khiển robot bám quỹ đạo kết hợp cảm biến EKF: 54 Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN: 61 VII 5.1 Kết luận: 61 5.2 Hướng phát triển: 61 Tài liệu tham khảo: 62 Phụ lục chương trình Matlab: 67 Mô lọc EKF cảm biến GPS giả lập: 67 Mô fusion EKF với odo GPS giả lập: 69 Mô lọc EKF cảm biến UWB thực tế: 71 Mô fusion với tín hiệu cảm biến odo UWB thực tế: 73 Mô điều khiển robot bám quỹ đạo fusion với tín hiệu cảm biến odo GPS giả lập tần số 100ms: 75 Mô điều khiển robot bám quỹ đạo fusion với tín hiệu cảm biến odo GPS giả lập tần số 1ms: 79 VIII MỤC LỤC HÌNH ẢNH: Hình Sơ đồ khối mơ tả sensor fusion Multisensor intergration Hình Kiểu phân loại Durant-Whyte dựa mối quan hệ nguồn vào Hình Sơ đồ kết hợp liệu kiểu JDL 10 Hình Mơ hình bên lọc Kalman 28 Hình Độ dịch chuyển robot dạng bánh 31 Hình Mơ tín hiệu giả lập cảm biến GPS Odometry 42 Hình 7: Mơ kết hợp cảm biến GPS Odometry EKF lọc tín hiệu GPS EKF 43 Hình Mơ kết hợp cảm biến GPS Odometry EKF lọc tín hiệu GPS EKF - đoạn thẳng 43 Hình Mơ kết hợp cảm biến GPS Odometry EKF lọc tín hiệu GPS EKF - góc rẽ 44 Hình 10 Modul DWM1000 45 Hình 11 Thời gian truyền (TOF) phương pháp two‐way ranging 46 Hình 12 Thuật tốn định vị Trilateral centroid 47 Hình 13 Robot Pioneer DX3 48 Hình 14 Quỹ đạo di chuyển robot 49 Hình 15: Tín hiệu thực tế cảm biến UWB Odometry 50 Hình 16 Kết lọc nhiễu tín hiệu UWB EKF 50 Hình 17 Kết fusing sensor UWB Odometry 51 Hình 18 So sánh kết fusing sensor UWB với Odometry EKF kết lọc UWB EKF 51 Hình 19 Mơ hình động học robot 52 Hình 20: Control block diagram 54 Hình 21 Tín hiệu cảm biến mơ q trình bám quỹ đạo robot với thời gian lấy mẫu 100ms 55 Hình 22: Kết mô điều khiển robot bám quỹ đạo kết hợp cảm biến EKF với thời gian lấy mẫu 100ms 56 Hình 23: Tracking error với thời gian lấy mẫu 100ms 57 Hình 24 Tín hiệu cảm biến mơ q trình bám quỹ đạo robot với thời gian lấy mẫu 100ms 58 Hình 25 Kết mô điều khiển robot bám quỹ đạo kết hợp cảm biến EKF với thời gian lấy mẫu 1ms 59 IX Hình 26 Kết mơ điều khiển robot bám quỹ đạo kết hợp cảm biến EKF với thời gian lấy mẫu 1ms – phóng to 59 Hình 27 Tracking error với thời gian lấy mẫu 1ms 60 Hình 28 Tracking error với thời gian lấy mẫu 1ms – phóng to 60 X plot (x_GPS_EKF,y_GPS_EKF,'LineWidth',1.5); legend('True','GPS','odo','GPS(EKF)') Mô fusion EKF với odo GPS giả lập: clear all; close all; clc; load('Quy_dao.mat'); dt= 10/1000; % 10 ms vx_r(1)=0.1; vy_r(1)=0; w_r(1)=0; v_r(1)=0.1; Q11= 15/1000; % sai so x Q22= 15/1000;% sai so y Q33= pi/(360*2);% sai so goc ex_z1=10/1000; ey_z1=10/1000; et_z1=pi/(360*2); e_xGPS=100/1000; e_xodo=20/1000; e_yGPS=100/1000; e_yodo=20/1000; e_the=pi/(360*2); % tin hieu cam bien: y=[x_GPS;x_odo;y_GPS;y_odo;the_z]; P=[ex_z1 0; ey_z1 0; 0 et_z1]; % ok R=[e_xGPS 0 0; e_xodo 0 0; 0 e_yGPS 0; 0 e_yodo 0; 0 0 e_the]; C=[1 0 0; 0 1 0; 0 0 1]'; I= [1 0; 0; 0 1]; 69 for i=2:size(xr,2) vx_r(i)=xr(i)-xr(i-1); vy_r(i)=yr(i)-yr(i-1); v_r(i)= sqrt(vx_r(i)^2+vy_r(i)^2); w_r(i)=ther(i)-ther(i-1); end X(:,1)=[xr(1); yr(1); ther(1)]; u(:,1)=[v_r(1);w_r(1) ]; u=[v_r;w_r]; d_the(1)=0; N= size(xr,2); e_xodo_m(1) = e_xodo; e_yodo_m(1) = e_yodo; for i=1:N-1 if i>1 e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo; e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo; end R=[e_xGPS 0 0; e_xodo_m(1,i) 0 0; 0 e_yGPS 0; 0 e_yodo_m(1,i) 0; 0 0 e_the]; B=[dt*cos(X(3,i)+d_the(i)/2) 0;dt*sin(X(3,i)+d_the(i)/2) 0; 1]; A=[1 -v_r(i)*dt*sin(X(3,i)); v_r(i)*dt*cos(X(3,i)); 0 1]; Q1=[Q11*dt+Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))^2; -Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i)); -Q33*(dt^2/2)*v_r(i)*sin(X(3,i))]; Q2=[-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i)); Q22*dt+Q33*(dt^3/3)*(v_r(i)^2)*cos(X(3,i))^2; Q33*(dt^2/2)*v_r(i)*cos(X(3,i))]; Q3=[-Q33*(dt^2/2)*v_r(i)*sin(X(3,i)); Q33*(dt^2/2)*v_r(i)*cos(X(3,i)); Q33*dt]; 70 Q=[Q1 Q2 Q3]; X_p(:,i+1)=X(:,i)+B*u(:,i); P_p=A*P*A'+Q; K= P_p*C'/(C*P_p*C'+R); X(:,i+1)= X_p(:,i+1)+K*(y(:,i+1)- C*X_p(:,i+1)); P=(I-K*C)*P_p; d_the(i+1)=X(3,i+1)-X(3,i); end x_fusion_EKF = X(1,:); y_fusion_EKF = X(2,:); figure (1); plot(xr,yr,x_GPS,y_GPS,x_odo,y_odo ); legend('True','GPS','Odo') figure (2); plot(xr,yr,x_fusion_EKF,y_fusion_EKF,x_odo,y_odo,x_GPS_EKF,y_GPS_EKF ); legend('True','(GPS+Odo)EKF','Odo','(GPS)EKF' ) Mô lọc EKF cảm biến UWB thực tế: clear all; close all; clc; % Loc UWB bang EKF load('du_lieu_UWB_Odo.mat') xr=x_ref; yr=y_ref; ther=the_ref; dt= 100/1000; % 100 ms vx_r(1)=0.1; vy_r(1)=0; w_r(1)=0; v_r(1)=0.1; Q11= 150/1000; % sai so x Q22= 150/1000;% sai so y Q33= pi/(360*2);% sai so goc ex_z1=1/1000; ey_z1=1/1000; 71 et_z1=pi/(360*2); y=[x_uwb_d1;y_uwb_d1;the_ref]; % ok P=[ex_z1 0; ey_z1 0; 0 et_z1]; R=[Q11 0; Q22 0; 0 Q33]; C=[1 0; 0; 0 1]; I= [1 0; 0; 0 1]; % tinh v_r va w_r for i=2:size(xr,2) vx_r(i)=xr(i)-xr(i-1); vy_r(i)=yr(i)-yr(i-1); v_r(i)= sqrt(vx_r(i)+vy_r(i)); w_r(i)=ther(i)-ther(i-1); end X(:,1)=[xr(1); yr(1); ther(1)]; u(:,1)=[v_r(1);w_r(1) ]; u=[v_r;w_r]; d_the(1)=0; N= size(xr,2); for i=1:N-1 B=[dt*cos(X(3,i)+d_the(i)/2) 0;dt*sin(X(3,i)+d_the(i)/2) 0; 1]; A=[1 -v_r(i)*dt*sin(X(3,i)); v_r(i)*dt*cos(X(3,i)); 0 1]; Q1=[Q11*dt+Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))^2; -Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i)); -Q33*(dt^2/2)*v_r(i)*sin(X(3,i))]; Q2=[-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i)); Q22*dt+Q33*(dt^3/3)*(v_r(i)^2)*cos(X(3,i))^2; Q33*(dt^2/2)*v_r(i)*cos(X(3,i))]; Q3=[-Q33*(dt^2/2)*v_r(i)*sin(X(3,i)); Q33*(dt^2/2)*v_r(i)*cos(X(3,i)); Q33*dt]; 72 Q=[Q1 Q2 Q3]; X_p(:,i+1)=X(:,i)+B*u(:,i); P_p=A*P*A'+Q; K= P_p*C/(C*P_p*C'+R); X(:,i+1)= X_p(:,i+1)+K*(y(:,i+1)- C*X_p(:,i+1)); P=(I-K*C)*P_p; d_the(i+1)=X(3,i+1)-X(3,i); end x_UWB_EKF = X(1,:); y_UWB_EKF = X(2,:); plot(xr,yr, x_uwb_d1,y_uwb_d1); hold on plot (x_UWB_EKF,y_UWB_EKF,'LineWidth',1.2); legend('True','UWB','(UWB)-EKF') Mơ fusion với tín hiệu cảm biến odo UWB thực tế: clear all; close all; clc; % Fusion UWB va odo load('du_lieu_UWB_Odo.mat') xr=x_ref; yr=y_ref; ther=the_ref; dt= 100/1000; % 100 ms vx_r(1)=0.1; vy_r(1)=0; w_r(1)=0; v_r(1)=0.1; Q11= 15/1000; Q22= 15/1000; Q33= pi/(360*2); ex_z1=10/1000; ey_z1=10/1000; et_z1=pi/(360*2); e_xUWB=150/1000; e_xodo=5/1000; 73 e_yUWB=150/1000; e_yodo=5/1000; e_the=pi/(360*2); y=[x_uwb_d1;x_odo_d1;y_uwb_d1;y_odo_d1;the_ref]; P=[ex_z1 0; ey_z1 0; 0 et_z1]; R=[e_xUWB 0 0; e_xodo 0 0; 0 e_yUWB 0; 0 e_yodo 0; 0 0 e_the]; C=[1 0 0; 0 1 0; 0 0 1]'; I= [1 0; 0; 0 1]; % tinh v_r va w_r for i=2:size(xr,2) vx_r(i)=xr(i)-xr(i-1); vy_r(i)=yr(i)-yr(i-1); v_r(i)= sqrt(vx_r(i)^2+vy_r(i)^2); w_r(i)=ther(i)-ther(i-1); end X(:,1)=[xr(1); yr(1); ther(1)]; u(:,1)=[v_r(1);w_r(1) ]; X_r=[xr;yr;ther]; u=[v_r;w_r]; d_the(1)=0; N= size(xr,2); e_xodo_m(1) = e_xodo; e_yodo_m(1) = e_yodo; for i=1:N-1 if i>1 74 e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo; e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo; end 0 0 R=[e_xUWB 0 0; e_xodo_m(1,i) 0 0; e_yUWB 0; 0 e_yodo_m(1,i) 0; 0 e_the]; B=[dt*cos(X(3,i)+d_the(i)/2) 0;dt*sin(X(3,i)+d_the(i)/2) 0; 1]; A=[1 -v_r(i)*dt*sin(X(3,i)); v_r(i)*dt*cos(X(3,i)); 0 1]; Q1=[Q11*dt+Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))^2; -Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i)); -Q33*(dt^2/2)*v_r(i)*sin(X(3,i))]; Q2=[-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i)); Q22*dt+Q33*(dt^3/3)*(v_r(i)^2)*cos(X(3,i))^2; Q33*(dt^2/2)*v_r(i)*cos(X(3,i))]; Q3=[-Q33*(dt^2/2)*v_r(i)*sin(X(3,i)); Q33*(dt^2/2)*v_r(i)*cos(X(3,i)); Q33*dt]; Q=[Q1 Q2 Q3]; X_p(:,i+1)=X(:,i)+B*u(:,i); P_p=A*P*A'+Q; K= P_p*C'/(C*P_p*C'+R); X(:,i+1)= X_p(:,i+1)+K*(y(:,i+1)- C*X_p(:,i+1)); P=(I-K*C)*P_p; d_the(i+1)=X(3,i+1)-X(3,i); end x_EKF_UWB_Odo = X(1,:); y_EKF_UWB_Odo = X(2,:); plot(xr,yr,x_odo_d1,y_odo_d1,x_uwb_d1,y_uwb_d1,x_EKF_UWB_Odo,y_EKF_UWB_ Odo); legend('True','Odo','UWB','(Odo+UWB)-EKF') Mô điều khiển robot bám quỹ đạo fusion với tín hiệu cảm biến odo GPS giả lập tần số 100ms: 75 close all clear all load('Quy_dao_100ms.mat'); %system dynamics hagv=0.230; %width of agv, m wagv=0.350; %length of agv, m b=0.3328; %wheel distance, m r=0.0975; %wheel radius, m d=0.0; %agv center to agv's axe, m bl=0.000; %line width %controler parameters k1=15; k2=120; k3=20; %sampling time and data time dt=0.1; t(1)=0; n=length(xr); %for initial values % tracking point xc(1)=xr(1)-0.010; yc(1)=yr(1)-0.010; phc(1)=phr(1)+10*pi/180; % AGV center ph(1)=phc(1); w(1)=0; v(1)=0; x(1)=xc(1)-d*cos(ph(1)); y(1)=yc(1)-d*sin(ph(1)); %=== for error of tracking === E0=[cos(ph(1)) sin(ph(1)) 0; -sin(ph(1)) cos(ph(1)) 0; 0 1]*[xr(1)xc(1); yr(1)-yc(1); phr(1)-phc(1)]; e1(1)=E0(1); e2(1)=E0(2); e3(1)=E0(3); %=== for wheel dynamics === vwl(1)=0; vwr(1)=0; e_xGPS=150/1000; e_xodo=3/1000; e_yGPS=150/1000; e_yodo=3/1000; e_the=pi/(360*2); 76 x_GPS(1)=x(1)+rand*e_xGPS; y_GPS(1)=y(1)+rand*e_yGPS; x_odo(1)=x(1); y_odo(1)=x(1); Q11= 150/1000; Q22= 150/1000; Q33= pi/(360*2); e_xodo_m(1) = e_xodo; e_yodo_m(1) = e_yodo; ex_z1=150/1000; ey_z1=150/1000; et_z1=pi/(360*2); P_k=[ex_z1 0; ey_z1 0; 0 et_z1]; C_k=[1 0 0; 0 1 0; 0 0 1]'; for i=2:n t(i)=(i-1)*dt; %error dynamics E=([-1;0;0]*v(i-1) + [e2(i-1);-d-e1(i-1);-1]*w(i-1) + [vr*cos(e3(i1)); vr*sin(e3(i-1)); wr(i-1)])*dt+E0; e1(i)=E(1); e2(i)=E(2); e2(i)=e2(i); e3(i)=E(3); E0=E; %controller dynamics w(i)=wr(i)+k2*vr*e2(i)+k3*sin(e3(i)); v(i)=vr*cos(e3(i))+k1*e1(i); %tracking point dynamics ph(i)=phr(i)-e3(i); Ae=[cos(ph(i)) sin(ph(i)) er=inv(Ae)*E; xc(i)=xr(i)-er(1); yc(i)=yr(i)-er(2); phc(i)=phr(i)-er(3); 0; -sin(ph(i)) phc_dot(i)=phc(i)-phc(i-1); %wheel dynamics [rpm] vwl(i)=(1/r)*(v(i)+b*w(i))*(60/(2*pi)); vwr(i)=(1/r)*(v(i)-b*w(i))*(60/(2*pi)); 77 cos(ph(i)) 0; 0 1]; %robot center x(i)=xc(i)-d*cos(phc(i)); y(i)=yc(i)-d*sin(phc(i)); %%%%%%%%%%% gia lap tin hieu cam bien %%%%%%%%%%%%%% q_t(:,i)=[ x(i);y(i);phc(i)]; % phuong trinh 61 x_GPS(i)=x(i)+rand*e_xGPS; y_GPS(i)=y(i)+rand*e_yGPS; q_GPS(:,i)=[x_GPS(i);y_GPS(i);phc(i)]; % phuong trinh 62 x_odo(i)=x_odo(i-1)+x(i)-x(i-1)+rand*e_xodo; y_odo(i)=y_odo(i-1)+y(i)-y(i-1)+rand*e_yodo; q_odo(:,i)=[x_odo(i);y_odo(i);phc(i)]; d_the(i)=phc_dot(i); %%%%%%%%%%% uoc tinh trang thai %%%%%%%%%%%%%% z(:,i)=[v(i);w(i)]; u_k=z; X=q_t; [A_k, B_k, Q_k] = ham_tinh_he_so(dt,d_the,u_k, X, Q11,Q22,Q33,i); mea(:,i)=[x_GPS(i);x_odo(i);y_GPS(i);y_odo(i);phc(i)]; if i>1 e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo; e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo; end R=[e_xGPS 0 0; e_xodo_m(1,i) 0 0; 0 e_yGPS 0; 0 e_yodo_m(1,i) 0; 0 0 e_the]; [q_kalman, P_k]=kalman_day_du(mea, X,u_k,P_k,A_k,B_k,C_k,Q_k,R,i); x(i)=q_kalman(1,i); y(i)=q_kalman(2,i); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 78 end Ref=[xr;yr;phr]; Robot=[x;y;phc]; error=Robot-Ref; plot(t,error(1,:),t,error(2,:),'-.',t,error(3,:),' '); xlabel('Time (s)'); ylabel('Tracking Error (m,rad)'); legend('e1','e2','e3'); figure; plot(x,y,'lineWidth',1.2); xlabel('X Coordinate (m)'); ylabel('Y Coordinate (m)'); axis equal; hold on; plot(xr,yr); legend('Robot','Ref'); figure plot(xr,yr,x_odo, y_odo,x_GPS, y_GPS ); xlabel('X Coordinate (m)'); ylabel('Y Coordinate (m)'); legend('Ref', 'odo','GPS'); Mô điều khiển robot bám quỹ đạo fusion với tín hiệu cảm biến odo GPS giả lập tần số 1ms: close all clear all load('Quy_dao_1ms.mat'); %system dynamics hagv=0.230; %width of agv, m wagv=0.350; %length of agv, m b=0.3328; %wheel distance, m r=0.0975; %wheel radius, m d=0.0; %agv center to agv's axe, m bl=0.000; %line width %controler parameters k1=15; k2=12; k3=200; %sampling time and data time dt=0.001; t(1)=0; n=length(xr); 79 %for initial values % tracking point xc(1)=xr(1)-0.010; yc(1)=yr(1)-0.010; phc(1)=phr(1)+10*pi/180; % AGV center ph(1)=phc(1); w(1)=0; v(1)=0; x(1)=xc(1)-d*cos(ph(1)); y(1)=yc(1)-d*sin(ph(1)); %=== for error of tracking === E0=[cos(ph(1)) sin(ph(1)) 0; -sin(ph(1)) cos(ph(1)) 0; 0 1]*[xr(1)xc(1); yr(1)-yc(1); phr(1)-phc(1)]; e1(1)=E0(1); e2(1)=E0(2); e3(1)=E0(3); %=== for wheel dynamics === vwl(1)=0; vwr(1)=0; e_xGPS=150/1000; e_xodo=0.1/1000; e_yGPS=150/1000; e_yodo=0.1/1000; e_the=pi/(360*2); x_GPS(1)=x(1)+rand*e_xGPS; y_GPS(1)=y(1)+rand*e_yGPS; x_odo(1)=x(1); y_odo(1)=x(1); Q11= 15/1000; Q22= 15/1000; Q33= pi/(360*2); e_xodo_m(1) = e_xodo; e_yodo_m(1) = e_yodo; ex_z1=15/1000; ey_z1=15/1000; et_z1=pi/(360*2); P_k=[ex_z1 0; ey_z1 0; 0 et_z1]; C_k=[1 0 0; 0 1 0; 0 0 1]'; 80 for i=2:n t(i)=(i-1)*dt; %error dynamics E=([-1;0;0]*v(i-1) + [e2(i-1);-d-e1(i-1);-1]*w(i-1) + [vr*cos(e3(i1)); vr*sin(e3(i-1)); wr(i-1)])*dt+E0; e1(i)=E(1); e2(i)=E(2); e2(i)=e2(i); e3(i)=E(3); E0=E; %controller dynamics w(i)=wr(i)+k2*vr*e2(i)+k3*sin(e3(i)); v(i)=vr*cos(e3(i))+k1*e1(i); %tracking point dynamics ph(i)=phr(i)-e3(i); Ae=[cos(ph(i)) sin(ph(i)) 0; -sin(ph(i)) cos(ph(i)) 1]; er=inv(Ae)*E; xc(i)=xr(i)-er(1); yc(i)=yr(i)-er(2); phc(i)=phr(i)-er(3); phc_dot(i)=phc(i)-phc(i-1); %wheel dynamics [rpm] vwl(i)=(1/r)*(v(i)+b*w(i))*(60/(2*pi)); vwr(i)=(1/r)*(v(i)-b*w(i))*(60/(2*pi)); %robot center x(i)=xc(i)-d*cos(phc(i)); y(i)=yc(i)-d*sin(phc(i)); %%%%%%%%%%% gia lap tin hieu cam bien %%%%%%%%%%%%%% q_t(:,i)=[ x(i);y(i);phc(i)]; x_GPS(i)=x(i)+rand*e_xGPS; y_GPS(i)=y(i)+rand*e_yGPS; q_GPS(:,i)=[x_GPS(i);y_GPS(i);phc(i)]; x_odo(i)=x_odo(i-1)+x(i)-x(i-1)+rand*e_xodo; y_odo(i)=y_odo(i-1)+y(i)-y(i-1)+rand*e_yodo; q_odo(:,i)=[x_odo(i);y_odo(i);phc(i)]; d_the(i)=phc_dot(i); %%%%%%%%%%% uoc tinh trang thai %%%%%%%%%%%%%% 81 0; 0 z(:,i)=[v(i);w(i)]; u_k=z; X=q_t; [A_k, B_k, Q_k] = ham_tinh_he_so(dt,d_the,u_k, X, Q11,Q22,Q33,i); mea(:,i)=[x_GPS(i);x_odo(i);y_GPS(i);y_odo(i);phc(i)]; if i>1 e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo; e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo; end R=[e_xGPS 0 0; e_xodo_m(1,i) 0 0; 0 e_yGPS 0; 0 e_yodo_m(1,i) 0; 0 0 e_the]; [q_kalman, P_k]=kalman_day_du(mea, X,u_k,P_k,A_k,B_k,C_k,Q_k,R,i); x(i)=q_kalman(1,i); y(i)=q_kalman(2,i); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end Ref=[xr;yr;phr]; Robot=[x;y;phc]; error=Robot-Ref; plot(t,error(1,:),t,error(2,:),'-.',t,error(3,:),' '); xlabel('Time (s)'); ylabel('Tracking Error (m,rad)'); legend('e1','e2','e3'); figure; plot(x,y,'lineWidth',1.2); xlabel('X Coordinate (m)'); ylabel('Y Coordinate (m)') axis equal; hold on; plot(xr,yr); legend('Robot','Ref'); figure 82 plot(xr,yr,x_odo, y_odo,x_GPS, y_GPS ); xlabel('X Coordinate (m)'); ylabel('Y Coordinate (m)') legend('Ref', 'odo','GPS'); 83