Tổng hợp bộ điều khiển trượt thích nghi cho phương tiện nổi tự hành trên cơ sở mạng nơ ron nhân tạo

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ TỔNG HỢP BỘ ĐIỀU KHIỂN TRƯỢT THÍCH NGHI CHO PHƯƠNG TIỆN NỔI TỰ HÀNH TRÊN CƠ SỞ MẠNG NƠ RON NHÂN TẠO Ngành: Kỹ thuật điều khiển tự động hóa Mã số: 52 02 16 LUẬN ÁN TIẾN SĨ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC: Hà Nội – 2023 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHỊNG VIỆN KHOA HỌC VÀ CƠNG NGHỆ QN SỰ BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ TỔNG HỢP BỘ ĐIỀU KHIỂN TRƯỢT THÍCH NGHI CHO PHƯƠNG TIỆN NỔI TỰ HÀNH TRÊN CƠ SỞ MẠNG NƠ RON NHÂN TẠO LUẬN ÁN TIẾN SĨ KỸ THUẬT Hà Nội – 2023 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHỊNG VIỆN KHOA HỌC VÀ CƠNG NGHỆ QN SỰ i LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu tơi Các số liệu, kết luận án trung thực chưa cơng bố cơng trình khác Các liệu tham khảo trích dẫn đầy đủ Tác giả luận án ii LỜI CẢM ƠN Tác giả xin bày tỏ lòng biết ơn sâu sắc tới thầy giáo hướng dẫn khoa học TS ln quan tâm, động viên, giúp đỡ, đóng góp ý kiến quý báu tạo điều kiện để tác giả thực hoàn thành luận án Xin chân thành cảm ơn Thủ trưởng Viện Khoa học Công nghệ Quân sự, Viện Tự động hóa Kỹ thuật Quân sự, Thủ trưởng cán phòng Đào tạo Viện Khoa học Công nghệ quân quan tâm giúp đỡ tác giả trình học tập nghiên cứu Tác giả chân thành cảm ơn nhà giáo, nhà khoa học, đồng nghiệp ngồi Viện Khoa học Cơng nghệ qn đóng góp ý kiến q trình thực luận án Cuối xin cảm ơn thành viên gia đình, đặc biệt vợ, bố mẹ tạo điều kiện thời gian, vật chất sát cánh động viên tinh thần để tác giả tập trung, cố gắng hoàn thành luận án Hà Nội, ngày … tháng … năm 2023 Nghiên cứu sinh iii MỤC LỤC Trang DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT v DANH MỤC CÁC BẢNG viii DANH MỤC HÌNH VẼ ix MỞ ĐẦU Chương TỔNG QUAN VỀ USV VÀ CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN 1.1 Các khái niệm 1.1.1 Các hệ trục tọa độ sử dụng phân tích phương tiện hàng hải 1.1.2 Vị trí hướng chuyển động tàu .8 1.2 Tổng quan xây dựng phương trình chuyển động phương tiện hàng hải 1.2.1 Phương trình chuyển động vật rắn 10 1.2.2 Một số mô hình phương tiện hàng hải 15 1.3 Phương tiện tự hành thiếu cấu chấp hành 18 1.3.1 Mơ hình động lực học USV thiếu cấu chấp hành 18 1.3.2 Biến đổi mô hình .22 1.3.3 Phân tích mơ hình 29 1.4 Mô hình nhiễu mơi trường .29 1.4.1 Dòng chảy .30 1.4.2 Gió 30 1.4.3 Sóng .32 1.5 Tình hình nghiên cứu ngồi nước 34 1.5.1 Tình hình nghiên cứu nước 34 1.5.2 Tình hình nghiên cứu ngồi nước 35 1.6 Đặt toán 39 1.7 Kết luận Chương 40 Chương TỔNG HỢP BỘ ĐIỀU KHIỂN TRƯỢT TẦNG THÍCH NGHI BÁM QUỸ ĐẠO TRÊN CƠ SỞ MẠNG NƠ RON NHÂN TẠO .41 2.1 Cơ sở lý thuyết .41 2.1.1 Phương pháp điều khiển trượt tầng 41 2.1.2 Mạng nơ ron RBF 46 2.2 Tổng hợp điều khiển trượt tầng thích nghi nơ ron cho USV 49 2.2.1 Tổng hợp điều khiển bám quỹ đạo cho USV .49 iv 2.2.2 Tổng hợp điều khiển trượt tầng thích nghi có xét đến sai lệch mơ hình nhiễu .55 2.2.3 Cấu trúc hệ thống điều khiển bám quỹ đạo trượt tầng thích nghi 62 2.3 Mơ kiểm chứng thuật toán .62 2.3.1 Kết mô điều khiển với quỹ đạo đường thẳng 63 2.3.2 Kết mô điều khiển với quỹ đạo đường cong 76 2.4 Kết luận Chương 87 Chương TỔNG HỢP BỘ QUAN SÁT TỐC ĐỘ THÍCH NGHI CHO USV TRÊN CƠ SỞ MẠNG NƠ RON NHÂN TẠO .89 3.1 Cơ sở lý thuyết .89 3.1.1 Bộ quan sát trạng thái Luenberger 90 3.1.2 Bộ quan sát tựa Luenberger 93 3.2 Thiết kế quan sát tốc độ thích nghi sở mạng nơ ron nhân tạo 94 3.3 Mô kiểm chứng điều khiển USV kết hợp quan sát vận tốc 106 3.3.1 Mô đánh giá chất lượng quan sát vận tốc thích nghi 106 3.3.2 Mô đánh giá chất lượng hệ thống điều khiển trượt tầng thích nghi kết hợp quan sát thích nghi 111 3.4 Kết luận Chương 114 KẾT LUẬN 115 DANH MỤC CÁC CƠNG TRÌNH KHOA HỌC ĐÃ CÔNG BỐ 117 TÀI LIỆU THAM KHẢO 118 PHỤ LỤC .125 v DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT Ký hiệu, chữ Đơn vị Ý nghĩa viết tắt x [m] Tọa độ phương tiện theo trục x y [m] Tọa độ phương tiện theo trục y z [m] Tọa độ phương tiện theo trục z  Véc tơ vị trí phương tiện u [m/s] Vận tốc tịnh tiến dọc thân v [m/s] Vận tốc tịnh tiến ngang r [rad/s] Vận tốc quay trở mũi USV  Véc tơ vận tốc USV  [rad] Góc lắc ngang USV   [rad] Góc lắc dọc USV [rad] Góc hướng mũi USV  Véc tơ lực mô men tác động lên USV X [N] Lực tác động theo phương x Y [N] Lực tác động theo phương y Z [N] Lực tác động theo phương z K [N.m] Mô men tác động theo trục x M [N.m] Mô men tác động theo trục y N [N.m] Mô men tác động theo trục z p [rad/s] Vận tốc góc theo trục x q [rad/s] Vận tốc góc theo trục y  [rad/s] Vận tốc góc theo trục z I M RB C Ma trận mơ men qn tính Ma trận qn tính hệ thống thân tàu Ma trận Coriolis ma trận hướng tâm phương tiện vi hàng hải CRB Ma trận Coriolis lực hướng tâm vật rắn CA Ma trận Coriolis lực hướng tâm thủy động lực D Ma trận suy giảm thủy động lực học DA Ma trận suy giảm thủy động lực học khối lượng nước kèm U Vận tốc tương đối phương tiện hàng hải J1 1  Ma trận quay chuyển đổi vận tốc dài J2 2  Ma trận quay chuyển đổi vận tốc góc   ,  Fˆ Các thành phần bất định mơ hình tàu nhiễu loạn từ mơi trường bên ngồi Hàm xấp xỉ thành phần bất định mơ hình tàu nhiễu loạn từ mơi trường bên ngồi e1 Sai lệch vị trí thành phần thứ e2 Sai lệch vận tơc thành phần thứ e3 Sai lệch vị trí thành phần thứ hai e4 Sai lệch vận tôc thành phần thứ hai W Ma trận trọng số mạng nơ ron DOF Bậc tự (Degrees Of Freedom) DP Định vị động (Dynamic Positioning) DSC Bộ điều khiển mặt động ECEF Hệ tọa độ gắn với mặt đất ECI HSMC AHSMC Hệ tọa độ quán tính Bộ điều khiển trượt tầng Bộ điều khiển trượt tầng thích nghi LQR Bộ điều khiển tối ưu tồn phương tuyến tính MPC Bộ điều khiển dự báo theo mơ hình vii NED Hệ tọa độ quy chiếu địa lý PID Bộ điều khiển PID RBF Hàm sở xuyên tâm (Radial basis function) SMC Bộ điều khiển trượt USV Phương tiện mặt nước tự hành (Unmanned surface vehicle) LOS SNAME QĐ Luật dẫn hướng theo đường ngắm (Light of Sight) Hiệp hội kỹ thuật hải quân hàng hải Quỹ đạo MNNs Mạng nơ ron nhiều lớp MIMO Hệ thống nhiều tín hiệu vào nhiều tín hiệu viii DANH MỤC CÁC BẢNG Trang Bảng 1.1: Các ký hiệu sử dụng phương tiện hàng hải 10 131 M1_mu = M_mu(1:2,:); M2_mu = M_mu(end,:); V = A*M1_mu + B*M2_mu; V = V\eye(2); e2 = er(1:2); e4 = er(end); %% Controlable signal to = -V*(A*F1 + A*lamda1*e2 + B*F2 + B*lamda2*e4 + k*S + delta*tanh(S)); end %% Sliding surface for controller function [sur,er, er_xy] = Sliding_surface(ST,SP,s_coef) x = ST(1:6,:); x_dot = ST(7:9,:); ref = SP(1:3); ref_dot = SP(4:6); A = s_coef(:,1:2); B = s_coef(:,3); lamda1 = s_coef(:,8:end); lamda2 = SP(end); e1 e2 e3 e4 = = = = x(1:2) - ref(1:2); x_dot(1:2) - ref_dot(1:2); x(3) - ref(3); x_dot(3) - ref_dot(3); s1 = lamda1*e1 + e2; s2 = lamda2*e3 + e4; S = A*s1 + B*s2; sur = [s1;s2;S]; er = [e2;e4]; er_xy=[e1;e3]; end ******************************************************************************* Chương trình mơ điều khiển trượt tầng thích nghi ******************************************************************* close all; clear; clc; %% Controller's parameters A = [9 0;0 3]; B = [0.1;1]; % Hai he so dung de dieu chinh to hop mat truot chung S=As1+Bs2 lamda1 = diag([0.3 2]); lamda2 = 1.5; %lamda1 lamda2 lua chon cho lamda1,2>0; k = diag([5 5]); delta = diag([5 5]); % he so dieu chinh de S -> Nen chon delta nho de tranh chaterring (k,delta > 0) %% Setup timer for simulation Ts = 1e-3; Tspan = 500; time = 0:Ts:Tspan; N = length(time); %% Initial conditions of USV's system x0 = [0;0;10;0;0;0]; x = [x0 zeros(6,N)]; to = zeros(2,N+1); %% Reference signal and water's velocities ref = zeros(3,N+1); ref_dot = zeros(3,N+1); ref_2dot = zeros(3,N+1); vc = zeros(3,N+1); vc_dot = zeros(3,N+1); vc_amp = [0.3;0.1]; for i = 1:(N+1) vc(:,i) = [vc_amp(1)*sin(0.7*i*Ts);0;vc_amp(2)*sin(0.5*i*Ts)]; vc_dot(:,i) = [vc_amp(1)*0.7*cos(0.7*i*Ts);0;vc_amp(2)*0.5*cos(0.5*i*Ts)]; % % end vc(:,i) = 0*[vc_amp(1);0;vc_amp(2)]; vc_dot(:,i) = 0; 132 ref(2,1) = x0(2); for i = 1:N t = i*Ts; ref(1,i) = 1*t; ref(3,i) = 1*t; ref_dot(1,i) = 1; ref_dot(3,i) = 1; ref(2,i) = atan(ref(3,i)/ref(1,i)); ref_dot(2,i) = (ref(1,i)*ref_dot(3,i) - ref(3,i)*ref_dot(1,i))/(ref(1,i)^2 + ref(3,i)^2); if i >= ref_2dot(2,i) = (ref_dot(2,i)-ref_dot(2,i-1))/Ts; end end %% Sliding surface s1 = zeros(2,N); s2 = zeros(1,N); S = s1; e1=zeros(2,N); e3=zeros(1,N);e_0 = zeros(1,N); %% Network's parameters N1 = 5; N2 = 5; c1 = 0.1; c2 = 0.5; L1 = diag(1:1:N1); %L1 = L1\eye(N1); L2 = diag(1:1:N2); %L2 = L2\eye(N2); W1_mu = ones(N1,2,N+1); W2_mu = ones(N2,N+1); %% Calculate v_mu = vc; x_dot = zeros(3,1); t_wave = load('Fossen_wave_2.mat','t_w'); for i = 1:N v_mu(:,i) = x(4:6,i); J = [cos(x(2,i)), -sin(x(2,i)), 0;sin(x(2,i)), cos(x(2,i)), 0; 0, 0, 1]; J = [J(:,1),J(:,3),J(:,2)]; J = [J(1,:);J(3,:);J(2,:)]; %% System's states and mathematical model if i >= x_dot = (x(1:3,i)-x(1:3,i-1))/Ts; end [F1,F2,F_x,B_x,M_mu] = USV_MathematicalModel(x(:,i),x_dot,vc(:,i),vc_dot(:,i),ref_2dot(:,i),t_wave.t_w (2,i)); %% Sliding surface for controller SP = [ref(1:3,i);ref_dot(1:3,i);lamda2]; ST = [x(:,i); x_dot]; [sur,er] = Sliding_surface(ST,SP,[A,B,k,delta,lamda1]); s1(:,i) = sur(1:2); s2(:,i) = sur(3); S(:,i) = sur(4:end); %% Nonlinear functional approximation by RBF neural network [F1_mu,F2_mu,W1_next,W2_next] = Approximate_NeuralNet(x(:,i),x_dot,W1_mu(:,:,i),W2_mu(:,i),S(:,i),[N1 N2 c1 c2],L1,L2,[A,B],Ts); W1_mu(:,:,i+1) = W1_next; W2_mu(:,i+1) = W2_next; %% Hierarchical Sliding-mode Controller to(:,i) = Hierarchical_SlidingMode_Controller(F1_mu,F2_mu,M_mu,[A,B,k,delta,lamda1],SP,S( :,i),er); %% Next-states and second-order derivation x(:,i+1) = x(:,i) + Ts*(F_x + B_x*to(:,i) + B_x*[t_wave.t_w(1,i);t_wave.t_w(3,i)]); 133 end %% Plot Figure % USV's x states - figures figure(1), hold on; plot(time,x(1,1:N),'red','linewidth',1.5); plot(time,ref(1,1:N),'black-','linewidth',1.5); xlabel('times (seconds)'); ylabel("USV's states x (m)"); legend('x (m)','x_d (m)'); % USV's Psi states - figures figure(2), hold on; plot(time,x(2,1:N),'blue','linewidth',1.5); plot(time,ref(2,1:N),'black-','linewidth',1.5); xlabel('times (seconds)'); ylabel("USV's states \psi (rad)"); legend('\psi (rad)','\psi_d (rad)'); % USV's y states - figures figure(3), hold on; plot(time,x(3,1:N),'green','linewidth',1.5); plot(time,ref(3,1:N),'black-','linewidth',1.5); xlabel('times (seconds)'); ylabel("USV's states y (m)"); legend('y (m)','y_d (m)'); % Van toc theo phuong x figure(4), hold on; plot(time,v_mu(1,1:N),'red','linewidth',1.5); xlabel('times (seconds)'); ylabel("u(m/s)"); title('Velocities'); legend('u(m/s)'); % Van toc goc figure(5), hold on; plot(time,v_mu(2,1:N),'green','linewidth',1.5); xlabel('times (seconds)'); ylabel("r(rad/s)"); title('Velocities'); legend('r(rad/s)'); % Van toc theo phuong y figure(6), hold on; plot(time,v_mu(3,1:N),'black','linewidth',1.5); xlabel('times (seconds)'); ylabel("v(m/s)"); title('Velocities'); legend('v(m/s)'); % Tin hieu dieu khien figure(7), hold on; subplot(2,1,1); plot(time,to(1,1:N),'red','linewidth',1.5); xlabel('times (seconds)'); ylabel("\tau_x(N)"); title('Control signal \tau_x(N)'); %legend('\tau_x(N)','\tau_z(N.m)'); subplot(2,1,2); plot(time,to(2,1:N),'blue','linewidth',1.5); xlabel('times (seconds)'); ylabel("\tau_z(N.m)"); title('Control signal \tau_z(N.m)'); %legend('\tau_x(N)','\tau_z(N.m)'); % X_Y Errors e_x = x(1,1:N)-ref(1,1:N); 134 e_y= x(3,1:N)-ref(3,1:N); % X error figure(8), hold on; plot(time,e_x(1,1:N),'blue','LineWidth',1.5); plot(time,e_0(1,1:N),'black ','LineWidth',1.5); xlabel('times (seconds)'); ylabel("e_x(m)"); title('e_x (m)'); legend('e_x(m)') % Y error; figure(9), hold on; plot(time,e_y(1,1:N),'blue', 'LineWidth',1.5); plot(time,e_0(1,1:N),'black ','LineWidth',1.5); xlabel('times (seconds)'); ylabel("e_y(m)"); title('e_y (m)'); legend('e_y(m)') % X Y States figure(10), hold on; plot(ref(3,1:N),ref(1,1:N),'black ','linewidth',1.5); plot(x(3,1:N),x(1,1:N),'red','linewidth',1.5); xlabel('y (m)'); ylabel("x (m)"); title('USV States (m)'); figure(11), hold on; subplot(3,1,1); plot(time,t_wave.t_w(1,1:N),'red','linewidth',1.5); legend('Xware(N)'); subplot(3,1,2); plot(time,t_wave.t_w(2,1:N),'blue','linewidth',1.5); legend('Yware(N)'); subplot(3,1,3); plot(time,t_wave.t_w(3,1:N),'black','linewidth',1.5); legend('Zware(N.m)'); xlabel('times (seconds)'); ylabel("X_ware(m/s)"); title('ware'); %% Save data save HSMC_Neuron_qdthang_noise; %% Unmanned Surface Vehicle's Mathematical Model function [F1,F2,F_x,B_x,M_mu] = USV_MathematicalModel(x,x_dot,vc,vc_dot,ref_2dot,xv) %% Set value for system parameters X_u = 25; Y_v = 0.5; Y_r = 3; N_v = 0.06; N_r = 0.02; X_udot = -2.25; Y_vdot = -23.13; Y_rdot = -1.31; N_vdot = -16.41; N_rdot = -2.79; X_uu = -70.92; Y_vv = -99.99; Y_vr = -5.49; Y_rv = -5.49; Y_rr = -8.8; N_vv = -5.49; N_vr = -8.8; N_rv = -8.8; N_rr = -3.49; m = 30; xG = 0.45; yG = 0.75; Iz = 4.1; %% Conditions and the veclocities of USV system v_mu = x(4:6); psi = x(2); psi_dot = x_dot(2); u = v_mu(1); v = v_mu(3); r = v_mu(2); %% Jacobian J = [cos(psi), -sin(psi), 0;sin(psi), cos(psi), 0; 0, 0, 1]; J = [J(:,1),J(:,3),J(:,2)]; %Hoan doi cot cua ma tran Jacobi 135 J = [J(1,:);J(3,:);J(2,:)]; %Hoan doi hang cua ma tran Jacobi %% Jacobian Derivate J_dot = [-sin(psi) -cos(psi); 0 0; cos(psi) -sin(psi)]*psi_dot; %% Mrb and Ma Mrb = [m, 0, -m*yG;0, m, m*xG;-m*yG, m*xG, Iz]; Ma = [-X_udot, 0, 0;0, -Y_vdot, -Y_rdot;0, -N_vdot, -N_rdot]; Mrb = [Mrb(:,1),Mrb(:,3),Mrb(:,2)]; %Hoan doi cot cuoi cua ma tran Mrb Mrb = [Mrb(1,:);Mrb(3,:);Mrb(2,:)]; %Hoan doi hàng cuoi cua ma tran Mrb Ma = [Ma(:,1),Ma(:,3),Ma(:,2)]; %Hoan doi cot cua ma tran Ma Ma = [Ma(1,:);Ma(3,:);Ma(2,:)]; %Hoan doi hang cuoi cua ma tran Ma Mrb = Mrb + Ma; %% Crb and Ca Crb = [0, 0, -m*(xG*r+v); 0, 0, -m*(yG*r-u); m*(xG*r+v), m*(yG*r-u), 0]; Ca = [0, 0, 2*(Y_vdot*v+0.5*(Y_rdot+N_vdot)*r);0, 0, -X_udot*u; 2*(-Y_vdot*v-0.5*(Y_rdot+N_vdot)*r), X_udot*u, 0]; Crb = [Crb(:,1),Crb(:,3),Crb(:,2)]; %Hoan doi cot cua ma tran Crb Crb = [Crb(1,:);Crb(3,:);Crb(2,:)]; %Hoan doi hang cua ma tran Crb Ca = [Ca(:,1),Ca(:,3),Ca(:,2)]; %Hoan doi cot cua ma tran Ca Ca = [Ca(1,:);Ca(3,:);Ca(2,:)]; %Hoan doi hang cua ma tran Ca %% D % ur = u - vc(1); vr = v - vc(3); D = -[X_u+X_uu*abs(u), 0, 0;0, Y_v+Y_vv*abs(v)+Y_vr*abs(r), Y_r+Y_rv*abs(v)+Y_rr*abs(r); 0, N_v+N_vv*abs(v)+N_vr*abs(r), N_r+N_rv*abs(v)+N_rr*abs(r)]; D = [D(:,1),D(:,3),D(:,2)]; %Hoan doi cot cua ma tran D D = [D(1,:);D(3,:);D(2,:)]; %Hoan doi hang cua ma tran D Crb = Crb + Ca + D; %% Na Delta = 1.5*[2*abs(u)^2+0.05;0;abs(r)^3 + 1*sin(v)]; N = -Ma*vc_dot - (Ca+D)*vc; N = N + Delta; %% Matrix block J1_dot = J_dot(1:2,1:2); J2_dot = J_dot(1:2,end); J3_dot = J_dot(end,1:2); J1 = J(1:2,1:2); J2 = J(1:2,end); J3 = J(end,1:2); J4 = J(end,end); J4_dot = J_dot(end,end); M1 = Mrb(1:2,1:2); M2 = Mrb(1:2,end); M3 = Mrb(end,1:2); M4 = Mrb(end,end); C1 = Crb(1:2,1:2); C2 = Crb(1:2,end); C3 = Crb(end,1:2); C4 = Crb(end,end); G1 = N(1:2); G2 = N(end) - xv; %% Mathematical Modeling M_gach = M1 - M2*(M4^(-1))*M3; f1 = -(M_gach\eye(2))*M2*(M4^(-1))*(C3*v_mu(1:2)+C4*v_mu(3)+G2) +(M_gach\eye(2))*(C1*v_mu(1:2)+C2*v_mu(3)+G1); f1 = -f1; f2 = -(M4^(-1))*M3*f1-(M4^(-1))*(C3*v_mu(1:2)+C4*v_mu(3)+G2); F1 = J1_dot*v_mu(1:2) + J2_dot*v_mu(3) + J1*f1 + J2*f2 - ref_2dot(1:2); F2 = J3_dot*v_mu(1:2) + J4_dot*v_mu(3) + J3*f1 + J4*f2 - ref_2dot(3); M1_mu = (J1-J2*(M4^(-1))*M3)*(M_gach\eye(2)); M2_mu = (J3-J4*(M4^(-1))*M3)*(M_gach\eye(2)); M_mu = [M1_mu;M2_mu]; F_x = [J1*v_mu(1:2)+J2*v_mu(3); J3*v_mu(1:2)+J4*v_mu(3); f1; f2]; B_x = [zeros(2,2); zeros(1,2); (M_gach\eye(2)); -(M4^(-1))*M3*(M_gach\eye(2))]; end %% Hierarchical Sliding-mode Controller 136 function to = Hierarchical_SlidingMode_Controller(F1_mu,F2_mu,M_mu,s_coef,SP,S,er) %% HSMC's parameters A = s_coef(:,1:2); B = s_coef(:,3); lamda1 = s_coef(:,8:end); k = s_coef(:,4:5); delta = s_coef(:,6:7); lamda2 = SP(end); %% Matrix block and satuaration M1_mu = M_mu(1:2,:); M2_mu = M_mu(end,:); V = A*M1_mu + B*M2_mu; V = V\eye(2); e2 = er(1:2); e4 = er(end); %% Controlable signal to = -V*(A*F1_mu + A*lamda1*e2 + B*F2_mu + B*lamda2*e4 + k*S + delta*tanh(S)); end %% Sliding surface for controller function [sur,er] = Sliding_surface(ST,SP,s_coef) x = ST(1:6,:); x_dot = ST(7:9,:); ref = SP(1:3); ref_dot = SP(4:6); A = s_coef(:,1:2); B = s_coef(:,3); lamda1 = s_coef(:,8:end); lamda2 = SP(end); e1 e2 e3 e4 = = = = x(1:2) - ref(1:2); x_dot(1:2) - ref_dot(1:2); x(3) - ref(3); x_dot(3) - ref_dot(3); s1 = lamda1*e1 + e2; s2 = lamda2*e3 + e4; S = A*s1 + B*s2; sur = [s1;s2;S]; er = [e2;e4]; end %% Approximate for F1 and F2 by RBF neural network function [F1_mu,F2_mu,W1_mu,W2_mu] = Approximate_NeuralNet(x,x_dot,W1_last,W2_last,S,Num_para,L1,L2,s_coef,Ts) N1 = Num_para(1); N2 = Num_para(2); c1 = Num_para(3); c2 = Num_para(4); A = s_coef(:,1:2); B = s_coef(:,3); %% First network parameters d1 = linspace(-.1,.1,N1); dd1 = linspace(-.1,.1,N1); d2 = linspace(-.1,.1,N1); dd2 = linspace(-.1,.1,N1); d3 = linspace(-.1,.1,N1); dd3 = linspace(-.1,.1,N1); d = [d1;d2;d3]; dd_1 = [dd1;dd2;dd3]; sigma1 = ones(1,N1); sigma2 = sigma1; %% Second network parameters t1 = linspace(-.10,.10,N2); dt1 = linspace(-.10,.10,N2); t2 = linspace(-.10,.10,N2); dt2 = linspace(-.10,.10,N2); t3 = linspace(-.10,.10,N2); dt3 = linspace(-.10,.10,N2); t = [t1;t2;t3]; dt_1 = [dt1;dt2;dt3]; eta1 = ones(1,N1); eta2 = sigma1; %% Active function h_temp1 = zeros(N1,1); h_temp2 = zeros(N2,1); h_sum1 = 0; h_sum2 = 0; h1 = h_temp1; h2 = h_temp2; for i = 1:N1 h_temp1(i) = ((x(1:3)' - d(:,i)')*(x(1:3) - d(:,i))/sigma1(i)^2) 137 +((x_dot' - dd_1(:,i)')*(x_dot - dd_1(:,i))/sigma2(i)^2) +((x(4:6)' - d(:,i)')*(x(4:6) - d(:,i))/sigma2(i)^2); h_temp1(i) = + exp(-h_temp1(i)); h_sum1 = h_sum1 + h_temp1(i); end for i = 1:N2 h_temp2(i) = ((x(1:3)' - t(:,i)')*(x(1:3) - t(:,i))/eta1(i)^2) +((x_dot'-dt_1(:,i)')*(x_dot-dt_1(:,i))/eta2(i)^2) +((x(4:6)' - t(:,i)')*(x(4:6) - t(:,i))/eta2(i)^2); h_temp2(i) = + exp(-h_temp2(i)); h_sum2 = h_sum2 + h_temp2(i); end for i = 1:N1 h1(i) = h_temp1(i); end for i = 1:N2 h2(i) = h_temp2(i); end %% Output % h1 = tanh(ones(N1,9)*[x;x_dot]); h2 = tanh(ones(N2,9)*[x;x_dot]); F1_mu = W1_last'*h1; F2_mu = W2_last'*h2; %% Calculate the weight of network W1_dot = -L1*c1*norm(S)*W1_last + L1*h1*S'*A; W2_dot = -L2*c2*norm(S)*W2_last + L2*h2*S'*B; W1_mu = W1_last + Ts*W1_dot; W2_mu = W2_last + Ts*W2_dot; end ************************************************************************* Chương trình mơ điều khiển trượt tầng thích nghi kết hợp quan sát: ****************************************************************** close all; %% Controller's parameters Ap = [5 0;0 2]; B = [0.1;1]; % Hai he so dung de dieu chinh to hop mat truot chung S=As1+Bs2 lamda1 = diag([0.3 2]); lamda2 = 1.8; %lamda1 lamda2 lua chon cho lamda1,2>0; lamda1 cang lon he xac lap cang nhanh k = diag([2 2]); delta = diag([2 2]); % he so dieu chinh de S -> Nen chon delta nho de tranh chaterring (k,delta > 0) %% Setup timer for simulation Ts = 1e-3; Tspan = 500; time = 0:Ts:Tspan; N = length(time); %% Initial conditions of USV's system x0 = [0;pi/3;1;0;0;0]; x = [x0 zeros(6,N)]; to = zeros(2,N+1); %% Initial conditions of USV's observer using neural network x_mu0 = [1;pi;2;-1;-3;1]; x_mu = [x_mu0, zeros(6,N)]; %% Output of USV's system and the observer y = x(1:3,:); y_mu = x_mu(1:3,:); ob_v = y_mu; %% Reference signal and water's velocities ref = zeros(3,N+1); ref_dot = zeros(3,N+1); ref_2dot = zeros(3,N+1); vc = zeros(3,N+1); vc_dot = zeros(3,N+1); vc_amp = [0.5;0.2]; for i = 1:(N+1) vc(:,i) = [vc_amp(1)*sin(0.7*i*Ts);0;vc_amp(2)*sin(0.5*i*Ts)]; 138 vc_dot(:,i) = [vc_amp(1)*0.7*cos(0.7*i*Ts);0;vc_amp(2)*0.5*cos(0.5*i*Ts)]; end ref(2,1) = x0(2); a = 0.0005; for i = 1:N t = i*Ts; ref(1,i) = t; ref(3,i) = sqrt(2*pi)*exp(-a*t^2); ref_dot(1,i) = 1; ref_dot(3,i) = -2*sqrt(2*pi)*t*a*exp(a*t^2); ref_2dot(1,i) = 0; ref_2dot(3,i) = -2*a*sqrt(2*pi)*exp(a*t^2)*(1-2*a*t^2); ref(2,i) = atan(ref_dot(3,i)/ref_dot(1,i)); ref_dot(2,i) = (ref_dot(1,i)*ref_2dot(3,i) ref_dot(3,i)*ref_2dot(1,i))/(ref_dot(1,i)^2 + ref_dot(3,i)^2); if i >= ref_2dot(2,i) = (ref_dot(2,i)-ref_dot(2,i-1))/Ts; end end %% Sliding surface s1 = zeros(2,N); s2 = zeros(1,N); S = s1; e1=zeros(2,N); e3=zeros(1,N);e_0 = zeros(1,N); %% Network's parameters N1 = 5; N2 = 5; c1 = 0.1; c2 = 0.1; L1 = diag(1:1:N1); %L1 = L1\eye(N1); L2 = diag(1:1:N2); %L2 = L2\eye(N2); % W1_mu = ones(N1,2,N+1); W2_mu = ones(N2,N+1); % N1 = 10; N2 = 10; c1 = 0.075; c2 = 0.075; % L1 = diag(1:1:N1); L2 = diag(1:1:N2); num_para = [N1 N2 c1 c2]; W1_mu_o = ones(N1,2,1); W1_mu = ones(N1,2,N+1); W2_mu = ones(N2,N+1); W1_mu(:,:,1) = W1_mu_o; plat_W1_mu = [W1_mu_o(:) ones(2*N1, N)]; %% Output matrix of system and matrix for observer A = [zeros(3) eye(3);diag([-50 -92 -50]) diag([-96 -69 -100])]; C = [eye(3) zeros(3)]; K = diag([120 120 120 120 2441 0])*C'; %% Parameters of observer and network's weight L = 10; Wo = ones(L,6); Yo = ones(9,L); kappa1 = 0.6; kappa2 = 0.4; theta1 = 5; theta2 = 2; Wo_norm = [norm(Wo) zeros(1,N)]; Yo_norm = [norm(Yo) zeros(1,N)]; %% Gaussian white noise w1 = wgn(1,N+1,randi([-70,-60],1,1),'real'); w2 = wgn(1,N+1,randi([-70,60],1,1),'real'); w3 = wgn(1,N+1,randi([-70,-60],1,1),'real'); w4 = wgn(1,N+1,randi([-70,60],1,1),'real'); w5 = wgn(1,N+1,randi([-70,-60],1,1),'real'); w6 = wgn(1,N+1,randi([-70,60],1,1),'real'); wave = [w1;w2;w3;w4;w5;w6]; %% Calculate v_mu = vc; x_dot = zeros(3,1); x_mu_last = x_mu(1:3,1); num = 0; d1 = 0; d2 = 0; d3 = 0; t_wave = load('Fossen_wave_2.mat','t_w'); for i = 1:N 139 v_mu(:,i) = x(4:6,i); J = Jacobi_Matrix(x(2,i)); %% System's states and mathematical model x_dot = J*v_mu(:,i); [F_x,B_x,M_mu] = USV_MathematicalModel(x(:,i),vc(:,i),vc_dot(:,i),t_wave.t_w(2,i)); %% Calculating the sliding surface for HSMC controller SP = [ref(1:3,i);ref_dot(1:3,i);lamda2]; ST = [[x(1:3,i); x_mu(4:6,i)]; x_dot]; [sur,er] = Sliding_surface(ST,SP,[Ap,B,lamda1]); s1(:,i) = sur(1:2); s2(i) = sur(3); S(:,i) = sur(4:end); %% Observated velocities J_mu = Jacobi_Matrix(x_mu(2,i)); ob_v(:,i) = (J_mu')*(x_mu(1:3,i)-x_mu_last)/Ts; x_mu_last = x_mu(1:3,i); %% Approximate for F1 and F2 by RBF neural network ST = [x(1:3,i);ob_v(:,i)]; [F1_mu,F2_mu,W1_next,W2_next] = Approximate_NeuralNet(ST,x_dot,S(:,i),W1_mu(:,:,i), W2_mu(:,i),num_para,[Ap,B],L1,L2,Ts); W1_mu(:,:,i+1) = W1_next; W2_mu(:,i+1) = W2_next; %% Hierarchical Sliding-mode Controller to(:,i) = Hierarchical_SlidingMode_Controller(F1_mu,F2_mu,M_mu,[Ap,B,k,delta,lamda1],SP,S (:,i),er); %% Neural-observation for state determining for USV z = [x_mu(:,i)', [to(:,i);0]']'; y_nga = y(:,i) - y_mu(:,i); x_mu_f = Neural_Observation_for_USV(x_mu(:,i),z,y_nga,[A K],Wo,Yo,Ts); [Wo_f,Yo_f] = Update_Weight_NeuralNet(Wo,Yo,z,y_nga,[A K],[kappa1;kappa2;theta1;theta2],L,Ts); %% Next-states and second-order derivation x(:,i+1) = x(:,i) + Ts*(F_x + B_x*(to(:,i)+ [t_wave.t_w(1,i);t_wave.t_w(3,i)])); x_mu(:,i+1) = x_mu_f; y(:,i+1) = C*x(:,i+1); y_mu(:,i+1) = C*x_mu(:,i+1); Wo = Wo_f; Yo = Yo_f; %% Save the W1, Wo_norm and Yo_norm Wo_norm(i+1) = norm(Wo); Yo_norm(i+1) = norm(Yo); plat_W1_mu(:,i+1) = W1_next(:); end %% Plot Figure % USV's x states - figures figure(1), hold on; plot(time,x(1,1:N),'red ','linewidth',1.5); plot(time,ref(1,1:N),'black-','linewidth',1.5); plot(time,x_mu(1,1:N),'green','linewidth',1.5) xlabel('times (seconds)'); ylabel("USV's states x (m)"); legend('x (m)','x_d (m)','x observated (m)'); % USV's Psi states - figures figure(2), hold on; plot(time,x(2,1:N),'blue ','linewidth',1.5); plot(time,ref(2,1:N),'black-','linewidth',1.5); plot(time,x_mu(2,1:N),'green','linewidth',1.5) 140 xlabel('times (seconds)'); ylabel("USV's states \psi (rad)"); legend('\psi (rad)','\psi_d (rad)','\psi observated (rad)'); % USV's y states - figures figure(3), hold on; plot(time,x(3,1:N),'green ','linewidth',1.5); plot(time,ref(3,1:N),'black-','linewidth',1.5); plot(time,x_mu(3,1:N),'red','linewidth',1.5) xlabel('times (seconds)'); ylabel("USV's states y (m)"); legend('y (m)','y_d (m)','y observated (m)'); % Van toc theo phuong x figure(4), hold on; plot(time,v_mu(1,1:N),'red ','linewidth',1.5); plot(time,ob_v(1,1:N),'green+','linewidth',1.5); xlabel('times (seconds)'); ylabel("u(m/s)"); title('Velocities'); legend('u(m/s)','u observated (m/s)'); % Van toc goc figure(5), hold on; plot(time,v_mu(2,1:N),'red ','linewidth',1.5); plot(time,ob_v(2,1:N),'black+','linewidth',1.5); xlabel('times (seconds)'); ylabel("r(rad/s)"); title('Velocities'); legend('r(rad/s)','r observated (m/s)'); % Van toc theo phuong y figure(6), hold on; plot(time,v_mu(3,1:N),'black ','linewidth',1.5); plot(time,ob_v(3,1:N),'green+','linewidth',1.5); xlabel('times (seconds)'); ylabel("v(m/s)"); title('Velocities'); legend('v(m/s)','v observated (m/s)'); % Tin hieu dieu khien figure(7), hold on; subplot(2,1,1); plot(time,to(1,1:N),'red','linewidth',1.5); xlabel('times (seconds)'); ylabel("\tau_x(N)"); title('Control signal \tau_x(N)'); %legend('\tau_x(N)','\tau_z(N.m)'); subplot(2,1,2); plot(time,to(2,1:N),'blue','linewidth',1.5); xlabel('times (seconds)'); ylabel("\tau_z(N.m)"); title('Control signal \tau_z(N.m)'); %legend('\tau_x(N)','\tau_z(N.m)'); % X_Y Errors e_x = x(1,1:N)-ref(1,1:N); e_y= x(3,1:N)-ref(3,1:N); % X error figure(8), hold on; plot(time,e_x(1,1:N),'blue','LineWidth',1.5); plot(time,e_0(1,1:N),'black ','LineWidth',1.5); xlabel('times (seconds)'); ylabel("e_x(m)"); title('e_x (m)'); legend('e_x(m)') 141 % Y error; figure(9), hold on; plot(time,e_y(1,1:N),'blue', 'LineWidth',1.5); plot(time,e_0(1,1:N),'black ','LineWidth',1.5); xlabel('times (seconds)'); ylabel("e_y(m)"); title('e_y (m)'); legend('e_y(m)') % X Y States figure(10), hold on; plot(ref(3,1:N),ref(1,1:N),'black ','linewidth',1.5); plot(x(3,1:N),x(1,1:N),'red ','linewidth',1.5); plot(x_mu(3,1:N),x_mu(1,1:N),'blue','linewidth',1.5); xlabel('y (m)'); ylabel("x (m)"); title('USV States (m)'); % Trong so mang neuron xap xi figure(11), plot(time,W2_mu(:,1:N),'linewidth',1.5); figure(12), plot(time,plat_W1_mu(:,1:N),'linewidth',1.5); % Trong so mang neuron quan sat figure(13), hold on; plot(time,Wo_norm(1:N),'red','linewidth',1.5); plot(time,Yo_norm(1:N),'blue','linewidth',1.5); % sai lech quan sat van toc u e_u=v_mu(1,1:N)-ob_v(1,1:N); e_r=v_mu(2,1:N)-ob_v(2,1:N); e_v=v_mu(3,1:N)-ob_v(3,1:N); figure(11), hold on; subplot(3,1,1); plot(time,t_wave.t_w(1,1:N),'red','linewidth',1.5); legend('Xware(N)'); subplot(3,1,2); plot(time,t_wave.t_w(2,1:N),'blue','linewidth',1.5); legend('Yware(N)'); subplot(3,1,3); plot(time,t_wave.t_w(3,1:N),'black','linewidth',1.5); legend('Zware(N.m)'); xlabel('times (seconds)'); ylabel("X_ware(m/s)"); title('ware'); %% Save Data save Quansat_Fossen; %% Unmanned Surface Vehicle's Mathematical Model function [F_x,B_x,M_mu] = USV_MathematicalModel(x,vc,vc_dot,xv) c = 0; %% Set value for system parameters X_u = 25 + c*rand(1,1); Y_v = 0.5 + c*rand(1,1); Y_r = + c*rand(1,1); N_v = 0.06 + c*rand(1,1); N_r = 0.02 + c*rand(1,1); X_udot = -2.25 + c*rand(1,1); Y_vdot = -23.13 + c*rand(1,1); Y_rdot = -1.31 + c*rand(1,1); N_vdot = -16.41 + c*rand(1,1); N_rdot = -2.79 + c*rand(1,1); X_uu = -70.92 + c*rand(1,1); Y_vv = -99.99 + c*rand(1,1); Y_vr = -5.49 + c*rand(1,1); 142 Y_rv = -5.49 + c*rand(1,1); Y_rr = -8.8 + c*rand(1,1); N_vv = -5.49 + c*rand(1,1); N_vr = -8.8 + c*rand(1,1); N_rv = -8.8 + c*rand(1,1); N_rr = -3.49 + c*rand(1,1); m = 30 + c*randi([-5,5],1,1); xG = 0.45 + c*randi([-5,5],1,1); yG = 0.75 + c*randi([-5,5],1,1); Iz = 4.1 + c*rand(1,1); %% Conditions and the veclocities of USV system v_mu = x(4:6); psi = x(2); u = v_mu(1); v = v_mu(3); r = v_mu(2); %% Jacobian J = [cos(psi), -sin(psi), 0;sin(psi), cos(psi), 0; 0, 0, 1]; J = [J(:,1),J(:,3),J(:,2)]; %Hoan doi cot cua ma tran Jacobi J = [J(1,:);J(3,:);J(2,:)]; %Hoan doi hang cua ma tran Jacobi %% Mrb and Ma Mrb = [m, 0, -m*yG;0, m, m*xG;-m*yG, m*xG, Iz]; Ma = [-X_udot, 0, 0;0, -Y_vdot, -Y_rdot;0, -N_vdot, -N_rdot]; Mrb = [Mrb(:,1),Mrb(:,3),Mrb(:,2)]; %Hoan doi cot cuoi cua ma tran Mrb Mrb = [Mrb(1,:);Mrb(3,:);Mrb(2,:)]; %Hoan doi hàng cuoi cua ma tran Mrb Ma = [Ma(:,1),Ma(:,3),Ma(:,2)]; %Hoan doi cot cua ma tran Ma Ma = [Ma(1,:);Ma(3,:);Ma(2,:)]; %Hoan doi hang cuoi cua ma tran Ma Mrb = Mrb + Ma; %% Crb and Ca Crb = [0, 0, -m*(xG*r+v); 0, 0, -m*(yG*r-u); m*(xG*r+v), m*(yG*r-u), 0]; Ca = [0, 0, 2*(Y_vdot*v+0.5*(Y_rdot+N_vdot)*r);0, 0, -X_udot*u; 2*(-Y_vdot*v-0.5*(Y_rdot+N_vdot)*r), X_udot*u, 0]; Crb = [Crb(:,1),Crb(:,3),Crb(:,2)]; %Hoan doi cot cua ma tran Crb Crb = [Crb(1,:);Crb(3,:);Crb(2,:)]; %Hoan doi hang cua ma tran Crb Ca = [Ca(:,1),Ca(:,3),Ca(:,2)]; %Hoan doi cot cua ma tran Ca Ca = [Ca(1,:);Ca(3,:);Ca(2,:)]; %Hoan doi hang cua ma tran Ca %% D D = -[X_u+X_uu*abs(u), 0, 0;0, Y_v+Y_vv*abs(v)+Y_vr*abs(r), Y_r+Y_rv*abs(v)+Y_rr*abs(r); 0, N_v+N_vv*abs(v)+N_vr*abs(r), N_r+N_rv*abs(v)+N_rr*abs(r)]; D = [D(:,1),D(:,3),D(:,2)]; %Hoan doi cot cua ma tran D D = [D(1,:);D(3,:);D(2,:)]; %Hoan doi hang cua ma tran D Crb = Crb + Ca + D; %% Na Delta = 1*[2*abs(u)^2+0.05;0;abs(r)^3 + sin(v)]; N = -Ma*vc_dot - (Ca+D)*vc; N = N + Delta; %% Matrix block J1 = J(1:2,1:2); J2 = J(1:2,end); J3 = J(end,1:2); J4 = J(end,end); M1 = Mrb(1:2,1:2); M2 = Mrb(1:2,end); M3 = Mrb(end,1:2); M4 = Mrb(end,end); C1 = Crb(1:2,1:2); C2 = Crb(1:2,end); C3 = Crb(end,1:2); C4 = Crb(end,end); G1 = N(1:2); G2 = N(end) - xv; %% Mathematical Modeling M_gach = M1 - M2*(M4^(-1))*M3; f1 = -(M_gach\eye(2))*M2*(M4^(-1))*(C3*v_mu(1:2)+C4*v_mu(3)+G2) +(M_gach\eye(2))*(C1*v_mu(1:2)+C2*v_mu(3)+G1); f1 = -f1; f2 = -(M4^(-1))*M3*f1-(M4^(-1))*(C3*v_mu(1:2)+C4*v_mu(3)+G2); M1_mu = (J1-J2*(M4^(-1))*M3)*(M_gach\eye(2)); M2_mu = (J3-J4*(M4^(-1))*M3)*(M_gach\eye(2)); M_mu = [M1_mu;M2_mu]; 143 F_x = [J1*v_mu(1:2)+J2*v_mu(3); J3*v_mu(1:2)+J4*v_mu(3); f1; f2]; B_x = [zeros(2,2); zeros(1,2); (M_gach\eye(2)); -(M4^(-1))*M3*(M_gach\eye(2))]; end %% Hierarchical Sliding-mode Controller function to = Hierarchical_SlidingMode_Controller(F1,F2,M_mu,s_coef,SP,S,er) %% HSMC's parameters A = s_coef(:,1:2); B = s_coef(:,3); lamda1 = s_coef(:,8:end); k = s_coef(:,4:5); delta = s_coef(:,6:7); lamda2 = SP(end); e2 = er(1:2); e4 = er(3); %% Matrix block and satuaration M1_mu = M_mu(1:2,:); M2_mu = M_mu(end,:); V = A*M1_mu + B*M2_mu; Vdet = 1/(V(1,1)*V(2,2)-V(1,2)*V(2,1)); V = Vdet*[V(2,2) -V(2,1);-V(1,2) V(1,1)]'; %% Controlable signal to = -V*(A*F1 + A*lamda1*e2 + B*F2 + B*lamda2*e4 + k*S + delta*tanh(S)); end %% Calculating the sliding surface for HSMC controller function [sur,er] = Sliding_surface(ST,SP,s_coef) x = ST(1:6,:); x_dot = ST(7:9,:); ref = SP(1:3); ref_dot = SP(4:6); A = s_coef(:,1:2); B = s_coef(:,3); lamda1 = s_coef(:,4:5); lamda2 = SP(end); e1 e2 e3 e4 = = = = x(1:2) - ref(1:2); x_dot(1:2) - ref_dot(1:2); x(3) - ref(3); x_dot(3) - ref_dot(3); s1 = lamda1*e1 + e2; s2 = lamda2*e3 + e4; S = A*s1 + B*s2; sur = [s1;s2;S]; er = [e2;e4]; end %% Approximate for F1 and F2 by RBF neural network function [F1_mu,F2_mu,W1_next,W2_next] = Approximate_NeuralNet(x,x_dot,S,W1_last,W2_last,num_para,s_coef,L1,L2,Ts) A = s_coef(:,1:2); B = s_coef(:,3); N1 = num_para(1); N2 = num_para(2); c1 = num_para(3); c2 = num_para(4); %% First network parameters d1 = linspace(-1,1,N1); dd1 = linspace(-1,1,N1); d2 = linspace(-1,1,N1); dd2 = linspace(-1,1,N1); d3 = linspace(-1,1,N1); dd3 = linspace(-1,1,N1); d = [d1;d2;d3]; dd_1 = [dd1;dd2;dd3]; sigma1 = ones(1,N1); sigma2 = sigma1; %% Second network parameters t1 = linspace(-1,1,N2); dt1 = linspace(-1,1,N2); t2 = linspace(-1,1,N2); dt2 = linspace(-1,1,N2); t3 = linspace(-1,1,N2); dt3 = linspace(-1,1,N2); t = [t1;t2;t3]; dt_1 = [dt1;dt2;dt3]; eta1 = 5*ones(1,N1); eta2 = sigma1; %% Active function 144 h_temp1 = zeros(N1,1); h_temp2 = zeros(N2,1); h_sum1 = 0; h_sum2 = 0; h1 = h_temp1; h2 = h_temp2; for i = 1:N1 h_temp1(i) = ((x(1:3)' - d(:,i)')*(x(1:3) - d(:,i))/sigma1(i)^2) +((x_dot' - dd_1(:,i)')*(x_dot - dd_1(:,i))/sigma2(i)^2) +((x(4:6)' - d(:,i)')*(x(4:6) - d(:,i))/sigma2(i)^2); h_temp1(i) = + exp(-h_temp1(i)); h_sum1 = h_sum1 + h_temp1(i); end for i = 1:N2 h_temp2(i) = ((x(1:3)' - t(:,i)')*(x(1:3) - t(:,i))/eta1(i)^2) +((x_dot'-dt_1(:,i)')*(x_dot-dt_1(:,i))/eta2(i)^2) +((x(4:6)' - t(:,i)')*(x(4:6) - t(:,i))/eta2(i)^2); h_temp2(i) = + exp(-h_temp2(i)); h_sum2 = h_sum2 + h_temp2(i); end for i = 1:N1 h1(i) = h_temp1(i); end for i = 1:N2 h2(i) = h_temp2(i); end %% Output F1_mu = W1_last'*h1; F2_mu = W2_last'*h2; %% Calculate the weight of network W1_dot = -L1*c1*norm(S)*W1_last + L1*h1*S'*A; W2_dot = -L2*c2*norm(S)*W2_last + L2*h2*S'*B; W1_next = W1_last + Ts*W1_dot; W2_next = W2_last + Ts*W2_dot; end %% Observer function x_mu_f = Neural_Observation_for_USV(x_mu,z,y_nga,Matrix_Para,Wo,Yo,Ts) A = Matrix_Para(:,1:6); K = Matrix_Para(:,7:end); x_mu_f = x_mu + Ts*(A*x_mu + Wo'*tanh(Yo'*z) + K*y_nga); end %% Update the weight of neural network function [Wo_f,Yo_f] = Update_Weight_NeuralNet(Wo,Yo,z,y_nga,Matrix,Parameters,L,Ts) kappa1 = Parameters(1); kappa2 = Parameters(2); theta1 = Parameters(3); theta2 = Parameters(4); A = Matrix(:,1:6); K = Matrix(:,7:9); C = [eye(3) zeros(3)]; Ao = A - K*C; F = eye(L,L); for i = 1:L F(i,i) = tanh(Yo(:,i)'*z); F(i,i) = F(i,i)^2; end Wo_dot = -kappa1*tanh(Yo'*z)*y_nga'*C*(Ao\eye(6)) - theta1*norm(y_nga)*Wo; Yo_dot = -kappa2*tanh(z)*y_nga'*C*(Ao\eye(6))*Wo'*(eye(L,L) - F) theta2*norm(y_nga)*Yo; Wo_f = Wo + Ts*Wo_dot; Yo_f = Yo + Ts*Yo_dot; 145 end %% Jacobian Matrix function J = Jacobi_Matrix(psi) J = [cos(psi), -sin(psi), 0;sin(psi), cos(psi), 0; 0, 0, 1]; J = [J(:,1),J(:,3),J(:,2)]; J = [J(1,:);J(3,:);J(2,:)]; end