Nghiên cứu điều khiển thích nghi cho robot lặn tự hành

% 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!

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','-')

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<<<<<<<<<<<