spsa_pd_tanker.m

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SPSA for Design of a
% Proportional-Derivative Controller for 
% Tanker Ship Heading Regulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% By: Kevin Passino 
% Version: 4/13/01
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear		% Clear all variables in memory


% Proportional and derivative gains (the input space for the design)

%Kp=-1.5; Some reasonable size gains - found manually
%Kd=-250;

Kpmin=-5;
Kpmax=-0.5;  % Program below assumes this

Kdmin=-500;
Kdmax=-100; % Program below assumes this


% Define scale parameters for performance measure
w1=1;
w2=0.01;

% Next, define SPSA parameters:

p=2;                 % Dimension of the search space 

thetamin=[Kpmin; Kdmin];  % Set edges of region want to search in
thetamax=[Kpmax;Kdmax];


Nspsa=50;            % Maximum number of iterations to produce

% Next set the parameters of the algorithm:

lambdap=1; % Use two step sizes due to scale differences in two dimensions
lambdad=500;
lambda0=1;

alpha1=0.602;
alpha2=0.101;
alpha1=0.602;  %Max of 1, min of 0.602
alpha2=0.101;  % Max of 1/6=0.1666666, min of 0.101

% Create two sizes since the sizes of P gain and D gain are quite different
cp=0.5;
cd=50;
		  
% Next, pick the initial value of the parameter vector

theta(:,1)=[-0.5; -300];  % Pick one that found was reasonable

% Allocate memory 

theta(:,2:Nspsa)=0*ones(p,Nspsa-1);
Jplus=zeros(Nspsa,1);
Jminus=zeros(Nspsa,1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the SPSA loop

for j=1:Nspsa

	% Use projection in case update (or initial values) out of range
	
	theta(:,j)=min(theta(:,j),thetamax);
	theta(:,j)=max(theta(:,j),thetamin);

	% Set parameters and perturb parameters
	
	lambdajp=lambdap/(lambda0+j)^alpha1;  
	lambdajd=lambdad/(lambda0+j)^alpha1;  
	cjp=cp/j^alpha2;  % Need two of these since have cp, cd
	cjd=cd/j^alpha2;
	Delta=2*round(rand(p,1))-1; % According to a Bernoulli +- 1 dist.
	thetaplus=theta(:,j)+[cjp*Delta(1,1); cjd*Delta(2,1)];
	thetaminus=theta(:,j)-[cjp*Delta(1,1); cjd*Delta(2,1)];
	
	% Use projection in case perturbed out of range.
	
	thetaplus=min(thetaplus,thetamax);
	thetaplus=max(thetaplus,thetamin);
	thetaminus=min(thetaminus,thetamax);
	thetaminus=max(thetaminus,thetamin);

	% Next we must compute the cost function for thetaplus and thetaminus
	
	Jplus(j,1)=pdtanker([thetaplus(1,1);thetaplus(2,1)],w1,w2);
	Jminus(j,1)=pdtanker([thetaminus(1,1);thetaminus(2,1)],w1,w2);
		
	% Next, compute the approximation to the gradient
	
	g=(Jplus(j,1)-Jminus(j,1))./(2*[cjp*Delta(1,1); cjd*Delta(2,1)]);

	% Then, update the parameters
	
	theta(:,j+1)=theta(:,j)-[lambdajp*g(1,1);lambdajd*g(2,1)];
		
end % End main loop...

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Provide a plot of the costs that were computed
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

t=0:Nspsa; 
tprime=0:Nspsa-1;

figure(1)
clf
plot(tprime,Jplus(:,1),'-',tprime,Jminus(:,1),'--')
xlabel('Iteration, j')
ylabel('Cost values')
title('Costs: J_p_l_u_s (solid) J_m_i_n_u_s (dashed)')


figure(2) 
clf
subplot(211)
plot(t,theta(1,:),'-')
ylabel('K_p')
title('PD controller parameters (K_p solid, K_d dashed)')
subplot(212)
plot(t,theta(2,:),'-')
xlabel('Iteration, j')
ylabel('K_d')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the best set of gains - just take to be last ones computed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Kpbest=theta(1,Nspsa+1)
Kdbest=theta(2,Nspsa+1)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	
%
% Next, we provide plots of the input and output of the ship 
% along with the reference heading that we want to track
% for the best design found.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialize ship parameters 
% (can test two conditions, "ballast" or "full"):

ell=350;			% Length of the ship (in meters)
u=5;				% Nominal speed (in meters/sec)
abar=1;             % Parameters for nonlinearity
bbar=1;

% Define the reference model (we use a first order transfer function 
% k_r/(s+a_r)):

a_r=1/150;
k_r=1/150;

flag=1;
%flag=0; % Test under off-nominal conditions

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the controller regulating the ship heading 

% Next, we initialize the simulation:

t=0; 		% Reset time to zero
index=1;	% This is time's index (not time, its index).  
tstop=1200;	% Stopping time for the simulation (in seconds) - normally 20000
step=1;     % Integration step size
T=10;		% The controller is implemented in discrete time and
			% this is the sampling time for the controller.
			% Note that the integration step size and the sampling
			% time are not the same.  In this way we seek to simulate
			% the continuous time system via the Runge-Kutta method and
			% the discrete time controller as if it were
			% implemented by a digital computer.  Hence, we sample
			% the plant output every T seconds and at that time
			% output a new value of the controller output.
counter=10;	% This counter will be used to count the number of integration
			% steps that have been taken in the current sampling interval.
			% Set it to 10 to begin so that it will compute a controller
			% output at the first step.
			% For our example, when 10 integration steps have been
			% taken we will then we will sample the ship heading
			% and the reference heading and compute a new output
			% for the controller.  
eold=0;     % Initialize the past value of the error (for use
            % in computing the change of the error, c).  Notice
            % that this is somewhat of an arbitrary choice since 
            % there is no last time step.  The same problem is
            % encountered in implementation.  
cold=0;     % Need this to initialize phiold below

psi_r_old=0; % Initialize the reference trajectory
ymold=0; 	 % Initial condition for the first order reference model

x=[0;0;0];	% First, set the state to be a vector            
x(1)=0;		% Set the initial heading to be zero
x(2)=0;		% Set the initial heading rate to be zero.  
			% We would also like to set x(3) initially but this
			% must be done after we have computed the output
			% of the controller.  In this case, by
			% choosing the reference trajectory to be 
			% zero at the beginning and the other initial conditions
			% as they are, and the controller as designed,
			% we will know that the output of the controller
			% will start out at zero so we could have set 
			% x(3)=0 here.  To keep things more general, however, 
			% we set the intial condition immediately after 
			% we compute the first controller output in the 
			% loop below.


% Next, we start the simulation of the system.  This is the main 
% loop for the simulation of the control system.

psi_r=0*ones(1,tstop+1);
psi=0*ones(1,tstop+1);
e=0*ones(1,tstop+1);
c=0*ones(1,tstop+1);
s=0*ones(1,tstop+1);
w=0*ones(1,tstop+1);
delta=0*ones(1,tstop+1);
ym=0*ones(1,tstop+1);

while t <= tstop

% First, we define the reference input psi_r  (desired heading).

if t>=0, psi_r(index)=0; end			    % Request heading of 0 deg
if t>=100, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>=1500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=3000, psi_r(index)=45*(pi/180); end    % Request heading of -45 deg
if t>=4500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=6000, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>=7500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=9000, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>=10500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=12000, psi_r(index)=45*(pi/180); end    % Request heading of -45 deg
if t>=13500, psi_r(index)=0; end    			% Request heading of 0 deg