% PLOT THE RESULTS
figure(1) clf subplot(211)
plot(t,psi_ref,'LineWidth',2,'Color','r','LineStyle','--') hold on
plot(t,psi,'LineWidth',2,'Color','b','LineStyle','-')
title('NNC control - Yaw angle psi (deg)'),ylabel('[Deg]'),grid on legend('Set course','Actual course','Location','Best')
%axes([0, -5, 600, 20])
subplot(212) plot(t,delta,'LineWidth',2,'Color','b','LineStyle','-') title('Rudder angle delta_R (deg)')
xlabel('Time (s)'),ylabel('[Deg]'),grid on
% legend('ANNAI controller','PID controller')
Có thể bạn quan tâm!
- Hướng Phát Triển Tiếp Theo Của Lĩnh Vực Nghiên Cứu
- Nghiên cứu điều khiển thích nghi cho robot lặn tự hành - 17
- Nghiên cứu điều khiển thích nghi cho robot lặn tự hành - 18
- Nghiên cứu điều khiển thích nghi cho robot lặn tự hành - 20
Xem toàn bộ 169 trang tài liệu này.
figure(2) clf subplot(311)
plot(t,Z_ref,'LineWidth',2,'Color','r','LineStyle','--') hold on plot(t,Z,'LineWidth',2,'Color','b','LineStyle','-')
title('NNC control - Actual depth Z (m)'),ylabel('[m]'),grid on legend('Desired depth','Actual depth','Location','Best') subplot(312) plot(t,theta,'LineWidth',2,'Color','b','LineStyle','-') title('Pitch angle theta (deg)'),ylabel('[Deg]'),grid on subplot(313) plot(t,delta_s,'LineWidth',2,'Color','b','LineStyle','-') title('Plane angle delta_S (deg)')
xlabel('Time (s)'),ylabel('[Deg]'),grid on
figure(3) clf subplot(211)
plot(t,ud,'LineWidth',2,'Color','r','LineStyle','--') hold on plot(t,u,'LineWidth',2,'Color','b','LineStyle','-')
title('NNC control - Actual speed U (m/s)'),ylabel('[m/s]'),grid on legend('Desired speed','Actual speed','Location','Best') subplot(212)
plot(t,n,'LineWidth',2,'Color','b','LineStyle','-') title('Propeller speed n (rpm)')
xlabel('Time (s)'),ylabel('[rpm]'),grid on
figure(4) clf
plot3(X,Y,Z,'LineWidth',2,'Color','b','LineStyle','-') grid on
title('Simulation of the AUV control by FC') xlabel('X(m)'),ylabel('Y(m)'),zlabel('Z(m)')
% >>>>>>>>>END<<<<<<<<<<<
PL3. File mô phỏng điều khiển dẫn đường AUV theo quỹ đạo
% Submarine control test
% Decoupled control of speed, depth and course
% Path following in Oxyz
% Date: 2009/May/01st
% Modified: 2019/Apr/14th
% Modified: 2021/Feb/19th
% - Heading control on Oxy axis
% - Depth control separately
% Written by: PHAM VIET ANH – HCM Univ. of Transport
%======================================================================
clear; % clear all variables
NN = 6000; % number of sample
h = 0.1; % sample time (sec) i = 1;
% -------------------------------------- z = 0;
z_s = 0;
z_n = 0;
delta = 0;
delta_s = 0;
n = 150;
u = 0.3;
max_delta = 20*(pi/180); max_delta_s = 20*(pi/180); max_n = 1500;
% Px = [0 0 50 50 0];
% Py = [0 50 50 0 0];
Px = [0 70 70 0 0 70 70];
Py = [0 0 70 70 0 0 70];
% Pz = [-10 -8 -8 -8 -10];
Pz = [-10 -10 -10 -10 -10 -10 -10];
% Pz = [-10 -10 -5 -10 -10 -10 -5];
% Psi = [135 45 315 225 135]*pi/180;
M = 2;
L = 5;
%----------------------------------------------------------------------
% Controller's parameters of the rudder
rho = 0.5; lambda = 0.1; sigma1 = 0.2; sigma2 = 0;
%----------------------------------------------------------------------
% Controller's parameters of the P&S stern planes
rho_s = 0.2; lambda_s = 0.1; sigma1_s = 0.2; sigma2_s = 0.01;
%----------------------------------------------------------------------
% Controller's parameters of the propeller
rho_n = 1.5; lambda_n = 0.1; sigma1_n = 0.2; sigma2_n = 0.05;
%----------------------------------------------------------------------
% ui = [ delta_r delta_s delta_b delta_bp delta_bs n ]'
% initial states: x = [ u v w p q r x y z phi theta psi ]' x = [0.3 0 0 0 0 0 -1 -1 -10 0 0 5*pi/180]';
U = 0.3; xd(1) = x(7);
yd(1) = x(8);
zd(1) = x(9);
psi1 = x(12); psi_d(1) = x(12); epsi(1) = 0;
Z_ref(1) = x(9);
dist2w = sqrt((Px(m) - xd(1))^2 + (Py(m) - yd(1))^2 + (Pz(m)-zd(1))^2);
%*************** START SIMULATION ******************* disp('Running...')
% We start executing the controller with ralative small weights: W21 = rand(6,4)*0.0001; % Set random hidden layer weights
W32 = rand(1,6)*0.0001; % Set random output layer weights W21_s = rand(6,4)*0.0001; % Set random hidden layer weights W32_s = rand(1,6)*0.0001; % Set random output layer weights W21_n = rand(6,4)*0.0001; % Set random hidden layer weights W32_n = rand(1,6)*0.0001; % Set random output layer weights
% lr_rate(3) = 0.01; % Initial learning rate
% N(3) = 5; % Initial number of iterations
net_in = zeros(4,1); % Set initial inputs of neural network net_in_s = zeros(4,1);
net_in_n = zeros(4,1);
while i < NN, i=i+1;
time = (i-1)*h; % simulation time in seconds clc;
fprintf('Running...n*Counter = %d (%d)n',round(i/10),round((NN- i)/10));
r = x(6);
w = x(3);
udot = u-x(1);
u = x(1);
q = x(5);
theta= x(11);
if dist2w <= 4.5*L,m=m+1;end
% Sensor noise for the heading sensor with a uniform distribution on
% [-0.1,+0.1] deg.
% s = 0.1*(pi/180)*(2*rand - 1);
s = 0; % This allow us to remove the noise psi = x(12) + s; % Heading of the ship with sensor noise
xd(i) = x(7);
yd(i) = x(8);
zd(i) = x(9);
% psi_d(i) = Psi(m);
dist2w = sqrt((Px(m) - xd(i))^2 + (Py(m) - yd(i))^2 + (Pz(m)- zd(i))^2);
% psi_d(i) = LOSguide(Px(m-1),Py(m-1),Px(m-1),Py(m-
1),Px(m),Py(m),2,L); % H??ng d?c theo các ?o?n
psi_d(i) = LOSguide(xd(i),yd(i),xd(i),yd(i),Px(m),Py(m),2,L); Z_ref(i) = Pz(m);
ud(i) = 0.4;
%--------------------------------------
% err = CourseError(psi_d(i),psi);
err = psi - psi_d(i); % Do lech huong di so voi huong dat
% Xu ly chuyen quadrant khi goc 0 va 360
if abs(err) > pi % Khi psi va psi_d nam o goc phan tu I va IV err = 2*pi - sign(err)*err;
end
net_in(1) = err;
%--------------------------------------
err_s = zd(i) - Z_ref(i); net_in_s(1) = err_s;
%--------------------------------------
err_n = -u + ud(i); net_in_n(1) = err_n;
%--------------------------------------
ii=round(i/10);
if round(i/10)==0,ii=2,end
% store data for presentation
xout(i,:) = [time,x',U,delta,delta_s,n]; e_out(ii,:) = [err; err_s;err_n];
%----------------------------------------------------------------------
----
% Dau ra cua BDK duoc tinh toan 1 giay 1 lan (theo bien "ii") if ii>3
% Calculate rudder command:
% delta = 1.25*err-0.015*x(6); % Kp=1, Kd=0.01 % PID
controller
delta = mlnnc(1,6,4,net_in,W21,W32); delta_s = mlnnc(1,6,4,net_in_s,W21_s,W32_s);
n = mlnnc(1,6,4,net_in_n,W21_n,W32_n)*1500;
if abs(delta)>max_delta,delta=sign(delta)*max_delta;end if
abs(delta_s)>max_delta_s,delta_s=sign(delta_s)*max_delta_s;end if abs(n)>max_n,n=sign(n)*max_n;end
ui = [delta
delta_s delta delta_s delta_s
n ];
[xdot,U] = npsauv(x,ui);
% numerical integration
x = euler2(xdot,x,h); % Euler integration z = z + h*err;
z_s = z_s + h*err_s; z_n = z_n + h*err_n;
% Now update the weights of NN using adaptive interaction algorithm
% (intensive training):
[W21,W32] = annaiTrainIte(h,0.5,50,1,6,4,net_in,... err,delta,r,z,rho,lambda,sigma1,sigma2);
% Add integration action
[W21_s,W32_s] = annaiTrainIte(h,0.5,50,1,6,4,net_in_s,...
err_s,delta_s,w,theta,rho_s,lambda_s,sigma1_s,sigma2_s); % Add integration action
[W21_n,W32_n] = annaiTrainIte(h,0.5,50,1,6,4,net_in_n,...
err_n,n/1500,udot,z_n,rho_n,lambda_n,sigma1_n,sigma2_n);% Add integration action
net_in(2) = e_out(ii-1,1); net_in(3) = e_out(ii-2,1); net_in(4) = e_out(ii-3,1);
net_in_s(2) = e_out(ii-1,2); net_in_s(3) = e_out(ii-2,2); net_in_s(4) = e_out(ii-3,2);
net_in_n(2) = e_out(ii-1,3); net_in_n(3) = e_out(ii-2,3); net_in_n(4) = e_out(ii-3,3);
end % end of "if ii>3"
%---------------------------------------------------------------------
end
% time-series
t = xout(:,1);
u = xout(:,2);
v = xout(:,3);
w = xout(:,4);
p = xout(:,5);
q = xout(:,6);
r = xout(:,7)*180/pi;
X = xout(:,8); X(1) = xd(1);
Y = xout(:,9); Y(1) = yd(1);
Z = xout(:,10);Z(1) = zd(1);
phi = xout(:,11);
theta = xout(:,12)*180/pi;
psi = xout(:,13)*180/pi; psi(1) = psi1*180/pi; U = xout(:,14);
delta = -xout(:,15)*180/pi; delta_s = xout(:,16)*180/pi; n = xout(:,17);
psi_ref = psi_d*180/pi;
for i=1:NN
if psi_ref(i)<90
psi_ref(i) = (90 - psi_ref(i)); else
psi_ref(i) = (5*90 - psi_ref(i));
end end
for i=1:NN if psi(i)<90
psi(i) = (90-psi(i)); else
psi(i) = (5*90 - psi(i));
end end
% PLOT THE RESULTS
figure(1) clf subplot(211)
plot(t,psi_ref,'LineWidth',2,'Color','r','LineStyle','--')
hold on plot(t,psi,'LineWidth',2,'Color','b','LineStyle','-')
title('NNC control - Yaw angle psi (deg)'),ylabel('[Deg]'),grid on legend('Set course','Actual course','Location','Best')
%axes([0, -5, 600, 20])
subplot(212) plot(t,delta,'LineWidth',2,'Color','b','LineStyle','-') title('Rudder angle delta_R (deg)')
xlabel('Time (s)'),ylabel('[Deg]'),grid on
% legend('ANNAI controller','PID controller')
figure(2) clf subplot(311)
plot(t,Z_ref,'LineWidth',2,'Color','r','LineStyle','--') hold on plot(t,Z,'LineWidth',2,'Color','b','LineStyle','-')
title('NNC control - Actual depth Z (m)'),ylabel('[m]'),grid on legend('Desired depth','Actual depth','Location','Best') subplot(312) plot(t,theta,'LineWidth',2,'Color','b','LineStyle','-') title('Pitch angle theta (deg)'),ylabel('[Deg]'),grid on subplot(313) plot(t,delta_s,'LineWidth',2,'Color','b','LineStyle','-') title('Plane angle delta_S (deg)')
xlabel('Time (s)'),ylabel('[Deg]'),grid on
figure(3) clf subplot(211)
plot(t,ud,'LineWidth',2,'Color','r','LineStyle','--') hold on plot(t,u,'LineWidth',2,'Color','b','LineStyle','-')
title('NNC control - Actual speed U (m/s)'),ylabel('[m/s]'),grid on legend('Desired speed','Actual speed','Location','Best') subplot(212)
plot(t,n,'LineWidth',2,'Color','b','LineStyle','-') title('Propeller speed n (rpm)')
xlabel('Time (s)'),ylabel('[rpm]'),grid on
figure(4) clf
% axis([-10 60 -10 60 -12 -8])
hold on plot3(X,Y,Z,'LineWidth',2,'Color','b','LineStyle','-') hold on plot3(Px,Py,Pz,'LineWidth',3,'Color','r','LineStyle','--') grid on
title('Simulation of the AUV track by NNC') xlabel('X(m)'),ylabel('Y(m)'),zlabel('Z(m)')
figure(5) clf
plot(X,Y,'LineWidth',2,'Color','b','LineStyle','-') hold on
grid on plot(Px,Py,'LineWidth',2,'Color','r','LineStyle','--') title('Simulation of the AUV track by NNC') xlabel('X(m)'),ylabel('Y(m)')
%>>>>>>>>>>>>END<<<<<<<<<<<<<
PL4. File LOSguide.m
function psi_los = LOSguide(pos_x,pos_y,x_k_1,y_k_1,x_k,y_k,n,L)
% LOSGUIDE calculate Light-Of-Sight guidance heading
%
% Inputs:
% pos_x current ship's position in X
% pos_y current ship's position in Y
% x_k_1 previuos way-point in X
% y_k_1 previuos way-point in Y
% x_k current way-point in X
% y_k current way-point in Y
% n number of ship length
% L ship's length
%
% Outputs:
% psi_los Light-Of-Sight guidance heading (rad)
%
% Reference: Morten Breivik (2003)-"Nonlinear Maneuvering Control of
% Underactuated Ships", MSc thesis, Norwegian University of
% Science and Technology, Department of Engineering
% Cybernetics, Trondheim, Norway
%
% Author: Phung-Hung Nguyen
% Date: 2005/Dec/4th
%=====================================================================
delta_x = x_k - x_k_1; delta_y = y_k - y_k_1;
if delta_x==0
x_los = x_k_1; % x_los = x_k if delta_y > 0
y_los = pos_y + n*L; else % delta_y < 0
y_los = pos_y - n*L;
end
else % delta_x~=0
D = delta_y/delta_x; E = x_k_1;
F = y_k_1;
G = -D*E + F;
A = 1 + D^2;
B = 2*(D*G - D*pos_y - pos_x);
C = pos_x^2 + pos_y^2 + G^2 - (n*L)^2 - 2*G*pos_y;
if delta_x > 0
x_los = (-B + sqrt(B^2 - 4*A*C))/(2*A); else % delta_x < 0
x_los = (-B - sqrt(B^2 - 4*A*C))/(2*A);
end
y_los = D*(x_los - E) + F; end % End of "if delta_x==0"
% Next, we calculate the psi_los (from 0 to 2*pi) if x_los == pos_x
if y_los > pos_y
psi_los = pi/2; else
psi_los = 3*(pi/2);
end
elseif x_los > pos_x
K = (y_los - pos_y)/(x_los - pos_x); if y_los > pos_y
psi_los = atan(K); elseif y_los < pos_y
psi_los = 2*pi + atan(K); else % if y_los == pos_y
psi_los = 0;
end
else % if x_los < pos_x
K = (y_los - pos_y)/(x_los - pos_x); if y_los ~= pos_y
psi_los = pi + atan(K);
% elseif y_los < pos_y
% psi_los = 2*pi + atan2(y_los - pos_y,x_los - pos_x); else % if y_los == pos_y
psi_los = pi; end
end
%>>>>>>>>>>>END<<<<<<<<<<<