%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 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 if t>=15000, psi_r(index)=45*(pi/180); end % Request heading of 45 deg if t>=16500, psi_r(index)=0; end % Request heading of 0 deg if t>=18000, psi_r(index)=45*(pi/180); end % Request heading of 45 deg if t>=19500, psi_r(index)=0; end % Request heading of 0 deg % Next, suppose that there is sensor noise for the heading sensor with that is % additive, with a uniform distribution on [- 0.01,+0.01] deg. s(index)=0.01*(pi/180)*(2*rand-1); %else %s(index)=0; % This allows us to remove the noise. %end psi(index)=x(1)+s(index); % Heading of the ship (possibly with sensor noise). if counter == 10, % When the counter reaches 10 then execute the % controller counter=0; % First, reset the counter % Reference model calculations: % The reference model is part of the controller and to simulate it % we take the discrete equivalent of the % reference model to compute psi_m from psi_r (if you use % a continuous-time reference model you will have to augment % the state of the closed-loop system with the state(s) of the % reference model and hence update the state in the Runge-Kutta % equations). % % For the reference model we use a first order transfer function % k_r/(s+a_r) but we use the bilinear transformation where we % replace s by (2/step)(z-1)/(z+1), then find the z-domain % representation of the reference model, then convert this % to a difference equation: ym(index)=(1/(2+a_r*T))*((2-a_r*T)*ymold+... k_r*T*(psi_r(index)+psi_r_old)); ymold=ym(index); psi_r_old=psi_r(index); % This saves the past value of the ym and psi_r so that we can use it % the next time around the loop % Controller calculations: e(index)=psi_r(index)-psi(index); % Computes error (first layer of perceptron) c(index)=(e(index)-eold)/T; % Sets the value of c eold=e(index); % Save the past value of e for use in the above % computation the next time around the loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % A proportional-derivative controller: delta(index)=Kpbest*e(index)+Kdbest*c(index); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% else % This goes with the "if" statement to check if the counter=10 % so the next lines up to the next "end" statement are executed % whenever counter is not equal to 10 % Now, even though we do not compute the controller at each % time instant, we do want to save the data at its inputs and output at % each time instant for the sake of plotting it. Hence, we need to % compute these here (note that we simply hold the values constant): e(index)=e(index-1); c(index)=c(index-1); delta(index)=delta(index-1); ym(index)=ym(index-1); end % This is the end statement for the "if counter=10" statement % Next, the Runge-Kutta equations are used to find the next state. % Clearly, it would be better to use a Matlab "function" for % F (but here we do not, so we can have only one program). time(index)=t; % First, we define a wind disturbance against the body of the ship % that has the effect of pressing water against the rudder %if flag==0, w(index)=0.5*(pi/180)*sin(2*pi*0.001*t); % This is an additive sine disturbance to % the rudder input. It is of amplitude of % 0.5 deg. and its period is 1000sec. %delta(index)=delta(index)+w(index); %end % Next, implement the nonlinearity where the rudder angle is saturated % 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) %if flag==0, %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 if flag==0, 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); % Next, comes the plant: % 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 % (note that here only an approximation to the method is implemented where % we do not compute the function at multiple points in the integration step size). 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); % Next, we provide plots showing the performance of the best design figure(3) clf subplot(211) 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(212) plot(time,delta,'k-') zoom grid on title('Rudder angle, output of controller (input to the ship), deg.') xlabel('Time, sec') figure(4) clf plot(time,psi-ym,'k-') zoom grid on title('Ship heading error between ship heading and reference model heading, deg.') xlabel('Time, sec') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Just repeat above code, but for off-nominal case % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % End of program % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%