vehiclecontrol.m

用于汽车巡航控制系统的模糊控制算法

function [varray, E, dedc, dedb, dedgamma, dedbeta] = ...
   VehicleControl(nin, nclassin, nout, nclassout, b, c, bout, cout, rule, DerivFlag)

% function [E, dedc, dedb, dedgamma, dedbeta] = ...
%    VehicleControl(nin, nclassin, nout, nclassout, b, c, bout, cout, rule, DerivFlag)
% Fuzzy vehicle cruise control software.

% INPUTS
%   nin = number of fuzzy input variables.
%   nclassin = nin-element vector containing number of classes for each
%              fuzzy input variable.
%   nout = number of fuzzy output variables.
%   nclassout = nout-element vector containing number of classes for each
%               fuzzy output variable.
%   b = (nin x max(nclassin) x 2)-array containing the lower and upper
%       half widths of the input membership functions.
%       The first subscript specifies the input variable.
%       The second subscript specifies the fuzzy class number.
%       The third subscript specifies the lower half width or upper half width.
%   c = (nin x max(nclassin))-array containing the centroids of
%       the input membership functions.
%       The first subscript specifies the input variable.
%       The second subscript specifies the fuzzy class number.
%   bout = (nout x max(nclassout) x 2)-array containing the lower and upper
%          half widths of the output membership functions.
%   cout = (nout x max(nclassout))-array containing the centroids of
%          the output membership functions.
%   rule = rule base, an (nin + 1)-dimension array, each element of which gives
%          the fuzzy output resulting from the input fuzzy values.  For instance,
%          rule(2,5,3)=4 means that if input #1 = 2 and input #2 = 5, then
%          output #3 will be in the 4th fuzzy output class.
%   DerivFlag = a flag (0 or 1) specifying if the derivatives of the fuzzy output
%               with respect to the membership function parameters should
%               be calculated.
% OUTPUTS
%   varray = time-dependent array of automobile velocities
%   E = error (summed over all time instants)
%   dedc = derivative of E with respect to input centroids.
%   dedb = derivative of E with respect to input half widths.
%   dedgamma = derivative of E with respect to output centroids.
%   dedbeta = derivative of E with respect to output half widths.

% gradeArray has one element for each grade change (degrees)
% that the user wants to simulate.
gradeArray = [10]; 
NumGrades = size(gradeArray, 2);

NormInDefault = [4 4];
NormOutDefault = [0.3];

% CONSTANTS
%   alpha = drag coefficient (newtons / (meters/sec)^2)
%   gamma = engine force coefficient (newtons)
%   Fi = engine idle force (newtons)
%   m = vehicle mass (kilograms)
alpha = 4;
gamma = 12500;
Fi = 1000;
m = 1000;

% Initialize the step size and final time (seconds) for the simulation.
dt = 0.25;
tf = 15;
N = tf / dt;
Nsquared = N * N;

% Set the maximum throttle position (radians).  
% The minimum throttle position is zero.
thetamax = 60 * pi / 180;

% If this was called from another m-file, do not plot the results.
% If this was called from Matlab's editor window without any parameters, 
% plot the results.
plotflag = ~exist('nin','var');
%if plotflag
%   close 'all';
%end

if ~exist('DerivFlag','var')
   DerivFlag = 0;
end

% Initialize the # of input variables, # of classes for each input, 
% # of output variables, # of classes for each output,
% input variable half widths, input variable centroids,
% output variable half widths, output variable centroids,
% and the rule base for the fuzzy controller.
if ~exist('nin','var')
   nclassin = [5 5];
   nclassout = [5];
   [nin, nout, b, c, bout, cout, rule] = ...
      FuzzInit(NormInDefault, NormOutDefault, nclassin, nclassout);
   % Check if the user wants to read the initial fuzzy parameters from an input file.
   sprompt = ['Enter the file name from which to read the fuzzy parameters.\n',...
         '(Enter a blank to use the default fuzzy parameters.)\n? '];
   filename = input(sprompt, 's');
   if ~isempty(filename)
      fid = fopen(filename, 'r');
      c = fscanf(fid, '%f', [nin, max(nclassin)]);
      b = fscanf(fid, '%f', [2 * nin, max(nclassin)]);
      b = reshape(b, [nin, max(nclassin), 2]);
      cout = fscanf(fid, '%f', [nout, max(nclassout)]);
      bout = fscanf(fid, '%f', [2 * nout, max(nclassout)]);
      bout = reshape(bout, [nout, max(nclassout), 2]);
      fclose(fid);
   end
end
dedc = zeros(nin, max(nclassin));
dedb = zeros(nin, max(nclassin), 2);
dedgamma = zeros(nout, max(nclassout));
dedbeta = zeros(nout, max(nclassout), 2);
E = 0;
maxe = 0;
maxde = 0;
mine = 0;
minde = 0;

for iGrade = 1 : NumGrades
   
   % Set the initial velocity (meters/sec), reference velocity (meters/sec),
   % velocity error (meters/sec), road grade (radians), and 
   % initial throttle position (radians). 
   v = 40;
   vref = 40;
   e = vref - v;
   grade = 0 * pi / 180; % This is the initial steady-state condition.
   theta = ((alpha * v * v * sign(v) + m * 9.81 * sin(grade) - Fi) / gamma)^2;
   grade = gradeArray(iGrade) * pi / 180;
	q = 0;
   varray = [v];
   thetaarray = [theta];
   for t = dt : dt : tf
      eold = e;
      e = vref - v;
      de = e - eold;
      maxe = max(e, maxe);
      mine = min(e, mine);
      maxde = max(de, maxde);
      minde = min(de, minde);
      x = [e de];
      % Use the fuzzy controller to computer the desired throttle position change.
      % Compute the partial derivatives that will be used to adjust the fuzzy
      % membership functions.
      [dtheta, dxhatdc, dxhatdb, dxhatdgamma, dxhatdbeta] = ...
         FuzzCalc(nin, nclassin, nout, nclassout, b, c, bout, cout, rule, x, DerivFlag);
      % Adjust the throttle position, which is constrained to be between 0 and thetamax.
      theta = theta + dtheta;
      theta = min(theta, thetamax);
      theta = max(theta, 0);
      % Compute the vehicle velocity at the next time instant based on the current
      % throttle position.
      v = Vehicle(theta, v, grade, dt, alpha, gamma, Fi, m);
      % Save data for plotting at the end of the simulation.
      varray = [varray v];
      thetaarray = [thetaarray theta];
      % Update the partial derivative information.
      q = q + 1;
      WeightFn = q * q / Nsquared;
      E = E + e * e * WeightFn;
      if DerivFlag
         dedc = dedc - e * dxhatdc * WeightFn;
         dedb = dedb - e * dxhatdb * WeightFn;
         dedgamma = dedgamma - e * dxhatdgamma * WeightFn;
         dedbeta = dedbeta - e * dxhatdbeta * WeightFn;
      end
   end
   
   if plotflag
      disp(['Error = ', num2str(E)]);
      t = 0 : dt : tf;
%      figure;
%      plot(t, thetaarray);
%      xlabel('Time (sec)');
%      ylabel('Control Input (radians)');
%      grid;
      figure;
      plot(t, varray);
      xlabel('Time (sec)');
      ylabel('Velocity (meters/sec)');
      axis([0 15 39.4 40.4]);
      grid;
   end
   
end
dedc = dedc / NumGrades / N;
dedb = dedb / NumGrades / N;
dedgamma = dedgamma / NumGrades / N;
dedbeta = dedbeta / NumGrades / N;
E = E / 2 / NumGrades / N;
if plotflag
	disp(['max e = ', num2str(maxe), ', min e = ', num2str(mine), ...
         ', max de = ', num2str(maxde), ', min de = ', num2str(minde)]);
end