fmrlc_tanker.m

一个用MATLAB编写的优化控制工具箱

% at +-80 degrees

if delta(index) >= 80*(pi/180), delta(index)=80*(pi/180); end
if delta(index) <= -80*(pi/180), delta(index)=-80*(pi/180); end

% The next line is used in place of the line following it to
% change the speed of the ship
if t>=1000000,
%if t>=9000,      % This switches the ship speed (unrealistically fast)
u=3; % A lower speed

else

u=5;

end

% Next, we change the parameters of the ship to tanker to reflect
% changing loading conditions (note that we simulate as if
% the ship is loaded while moving, but we only change the parameters
% while the heading is zero so that it is then similar to re-running
% the simulation, i.e., starting the tanker operation at different 
% times after loading/unloading has occurred).

% The next line is used in place of the line following it to keep
% "ballast" conditions throughout the simulation
if t>=1000000,
%if t>=9000,      % This switches the parameters in the middle of the simulation
K_0=0.83;  		% These are the parameters under "full" conditions
tau_10=-2.88;
tau_20=0.38;
tau_30=1.07;

else

K_0=5.88;		% These are the parameters under "ballast" conditions
tau_10=-16.91;
tau_20=0.45;
tau_30=1.43;

end

% The following parameters are used in the definition of the tanker model:

K=K_0*(u/ell);
tau_1=tau_10*(ell/u);
tau_2=tau_20*(ell/u);
tau_3=tau_30*(ell/u);

% Now, for the first step, we set the initial condition for the
% third state x(3).

if t==0, x(3)=-(K*tau_3/(tau_1*tau_2))*delta(index); end

% Next, we use the formulas to implement the Runge-Kutta method

F=[ x(2) ;
    x(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(x(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*x(2)^3 + bbar*x(2)) + (K/(tau_1*tau_2))*delta(index) ];
        
	k1=step*F;
	xnew=x+k1/2;

F=[ xnew(2) ;
    xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
   
	k2=step*F;
	xnew=x+k2/2;

F=[ xnew(2) ;
    xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
   
	k3=step*F;
	xnew=x+k3;

F=[ xnew(2) ;
    xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
   
	k4=step*F;
	x=x+(1/6)*(k1+2*k2+2*k3+k4); % Calculated next state

t=t+step;  			% Increments time
index=index+1;	 	% Increments the indexing term so that 
					% index=1 corresponds to time t=0.
counter=counter+1;	% Indicates that we computed one more integration step

end % This end statement goes with the first "while" statement 
    % in the program so when this is complete the simulation is done.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, we provide plots of data from the simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% First, we convert from rad. to degrees
psi_r=psi_r*(180/pi);
psi=psi*(180/pi);
delta=delta*(180/pi);
e=e*(180/pi);
c=c*(180/pi);
ym=ym*(180/pi);
ye=ye*(180/pi);

% Next, we provide plots 

figure(1)
clf
subplot(311)
plot(time,psi,'k-',time,ym,'k--',time,psi_r,'k-.')
zoom
grid on
title('Ship heading (solid) and desired ship heading (dashed), deg.')
subplot(312)
plot(time,delta,'k-')
zoom
grid on
title('Rudder angle, output of fuzzy controller (input to the ship), deg.')
subplot(313)
plot(time,p,'k-')
zoom
grid on
xlabel('Time (sec)')
title('Fuzzy inverse model output (nonzero values indicate adaptation)')

figure(2)
clf
subplot(211)
plot(time,e,'k-')
zoom
grid on
title('Ship heading error between ship heading and desired heading, deg.')
subplot(212)
plot(time,c,'k-')
zoom
grid on
xlabel('Time (sec)')
title('Change in ship heading error, deg./sec')

figure(3)
clf
subplot(211)
plot(time,ye,'k-')
zoom
grid on
title('Ship heading error between ship heading and reference model heading, deg.')
subplot(212)
plot(time,yc,'k-')
zoom
grid on
xlabel('Time (sec)')
title('Change in heading error between output and reference model, deg./sec')

end % This ends the if statement (on flag1) on whether you want to do a simulation
    % or just see the control surface
	
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, provide a plot of the fuzzy controller surface:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Request input from the user to see if they want to see the tuned
% controller mapping:

flag2=input('Do you want to see the nonlinear \n mapping implemented by the tuned fuzzy \n controller? \n (type 1 for yes and 0 for no) ');

if flag2==1,
	
% First, compute vectors with points over the whole range of 
% the fuzzy controller inputs plus 20% over the end of the range
% and put 100 points in each vector

e_input=(-(1/g1)-0.2*(1/g1)):(1/100)*(((1/g1)+0.2*(1/g1))-(-(1/g1)-...
     0.2*(1/g1))):((1/g1)+0.2*(1/g1)); 
ce_input=(-(1/g2)-0.2*(1/g2)):(1/100)*(((1/g2)+0.2*(1/g2))-(-(1/g2)-...
     0.2*(1/g2))):((1/g2)+0.2*(1/g2));

% Next, compute the fuzzy controller output for all these inputs

for jj=1:length(e_input) 
	for ii=1:length(ce_input) 

c_count=0;,e_count=0;   % These are used to count the number of 
						% non-zero mf certainities of e and c

% The following if-then structure fills the vector mfe 
% with the certainty of each membership fucntion of e for the 
% current input e.  We use triangular membership functions.

	if e_input(jj)<=ce(1)		% Takes care of saturation of the left-most 
					            % membership function
         mfe=[1 0 0 0 0 0 0 0 0 0 0]; % i.e., the only one on is the 
         							  % left-most one
	 e_count=e_count+1;,e_int=1; 	  %  One mf on, it is the 
	 								  % left-most one.
	elseif e_input(jj)>=ce(nume)				  % Takes care of saturation 
									  % of the right-most mf
	 mfe=[0 0 0 0 0 0 0 0 0 0 1];
	 e_count=e_count+1;,e_int=nume; % One mf on, it is the 
	 								% right-most one
	else      % In this case the input is on the middle part of the 
			  % universe of discourse for e
			  % Next, we are going to cycle through the mfs to 
			  % find all that are on
	   for i=1:nume
		 if e_input(jj)<=ce(i)
		  mfe(i)=max([0 1+(e_input(jj)-ce(i))/we]);   
		  				% In this case the input is to the 
		  				% left of the center ce(i) and we compute
						% the value of the mf centered at ce(i)
						% for this input e
			if mfe(i)~=0	
				% If the certainty is not equal to zero then say 
				% that have one mf on by incrementing our count
			 e_count=e_count+1;
			 e_int=i;	% This term holds the index last entry 
			 			% with a non-zero term
			end
		 else 
		  mfe(i)=max([0,1+(ce(i)-e_input(jj))/we]); 
		  						% In this case the input is to the
		  						% right of the center ce(i)
			if mfe(i)~=0
			 e_count=e_count+1;   
			 e_int=i;  % This term holds the index of the 
			 		   % last entry with a non-zero term
			end
		 end
		end
	end

% The following if-then structure fills the vector mfc with the 
% certainty of each membership fucntion of the c
% for its current value.

	if ce_input(ii)<=cc(1)	% Takes care of saturation of left-most mf
         mfc=[1 0 0 0 0 0 0 0 0 0 0];
	 c_count=c_count+1;
	 c_int=1;   
	elseif ce_input(ii)>=cc(numc)	
				        % Takes care of saturation of the right-most mf
	 mfc=[0 0 0 0 0 0 0 0 0 0 1];
	 c_count=c_count+1; 
	 c_int=numc;
	else
		for i=1:numc
		 if ce_input(ii)<=cc(i)  
		  mfc(i)=max([0,1+(ce_input(ii)-cc(i))/wc]);
			if mfc(i)~=0
			 c_count=c_count+1;
			 c_int=i;  	    % This term holds last entry 
			 		     	% with a non-zero term
			end
		 else 
		  mfc(i)=max([0,1+(cc(i)-ce_input(ii))/wc]);
			if mfc(i)~=0
			 c_count=c_count+1;
			 c_int=i;	% This term holds last entry 
			 			% with a non-zero term
			end
		 end
		end
	end

% The next loops calculate the crisp output using only the non-
% zero premise of error,e, and c.  

num=0;
den=0;     
	for k=(e_int-e_count+1):e_int  
						% Scan over e indices of mfs that are on
		for l=(c_int-c_count+1):c_int	
						% Scan over c indices of mfs that are on
		  prem=min([mfe(k) mfc(l)]); 
		  				% Value of premise membership function
% This next calculation of num adds up the numerator for the 
% center of gravity defuzzification formula. 
		  num=num+rules(k,l)*base*(prem-(prem)^2/2);
		  den=den+base*(prem-(prem)^2/2);		
% To do the same computations, but for center-average defuzzification,
% use the following lines of code rather than the two above:
%		num=num+rules(k,l)*prem;
%		den=den+prem;
		end
	end

   delta_output(ii,jj)=num/den;   
   			% Crisp output of fuzzy controller that is the input
   			% to the plant.
			
	end
end

% Convert from radians to degrees:

delta_output=delta_output*(180/pi);
e_input=e_input*(180/pi);
ce_input=ce_input*(180/pi);

% Plot the data

figure(4)
clf
surf(e_input,ce_input,delta_output);
view(145,30);
colormap(white);
xlabel('Heading error (e), deg.');
ylabel('Change in heading error (c), deg.');
zlabel('Fuzzy controller output (\delta), deg.');
title('FMRLC-tuned fuzzy controller mapping between inputs and output');

% Next, print out to the screen the rule base (output centers)

rules

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of program %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%