铰接式车辆的横向动力学仿真提供车辆模型研究（Matlab代码实现）

%% Maneuver
% The maneuver to be estimated by the Kalman Filter is defined here.
%
% Choosing simulation parameters:
%
T       = 6;                        % Total simulation time [s]
resol   = 50;                       % Resolution
TSPAN   = 0:T/resol:T;              % Time span             [s]
%%
% Running simulation.
%
simulatorPlant = Simulator(VehiclePlant, TSPAN);
simulatorPlant.dPSI0 = 0.35;
simulatorPlant.Simulate
%%
% Printing simulation parameters.
%
disp(simulatorPlant)
%%
% Retrieving states
%
XTPlant     = simulatorPlant.XT;
YTPlant     = simulatorPlant.YT;
PSIPlant    = simulatorPlant.PSI;
vTPlant     = simulatorPlant.VEL;
ALPHATPlant = simulatorPlant.ALPHAT;
dPSIPlant   = simulatorPlant.dPSI;
XOUTPlant   = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];
%%
% Generating graphics of the vehicle in the considered maneuver (plant)
%
gPlant = Graphics(simulatorPlant);
gPlant.TractorColor = 'r';
gPlant.Frame();
%%
%
close all                   % Closing figures
%% Modelo
% O modelo utilizado no algoritmo de estima玢o � baseado no mesmo modelo
% f韘ico considerado no modelo da planta. Al閙 disso, o modelo parte da
% premissa de que o projetista do estimador n鉶 tem conhecimento adequado
% da curva caracter韘tica do pneu. Logo, o modelo de ve韈ulo � igual ao
% utilizado na planta, por閙, o modelo de pneu � dado pelo modelo linear
% Tire linear, resultando num modelo do sistema de menor complexidade (em
% rela玢o � planta) e com mais hip髏eses simplificadoras.
%
% Inicializando o pneu
%
TireModel = TireLinear;
disp(TireModel)
%%
% Choosing model vehicle
%
VehicleModel        = VehicleSimpleNonlinear; % Same as plant
VehicleModel.tire   = TireModel;
disp(VehicleModel)
%%
% Simulador com o mesmo vetor TSPAN e simula玢o
%
simulatorModel = Simulator(VehicleModel, TSPAN);
simulatorModel.dPSI0 = 0.35;
simulatorModel.Simulate;
disp(simulatorModel)
%%
% Retrieving states
%
XTModel = simulatorModel.XT;
YTModel = simulatorModel.YT;
PSIModel = simulatorModel.PSI;
vTModel = simulatorModel.VEL;
ALPHATModel = simulatorModel.ALPHAT;
dPSIModel = simulatorModel.dPSI;
%%
% A manobra gerada pelo modelo escolhido pelo projetista a partir da mesma
% condi玢o inicial � ilustrada na figura a seguir
%
gModel = Graphics(simulatorModel);
gModel.TractorColor = 'g';
gModel.Frame();
%%
%
close all                   % Closing figures
%% Plant and model comparison
% Comparando o modelo de pneu
%
g       = 9.81;
FzF     = VehiclePlant.mF0*g;
FzR     = VehiclePlant.mR0*g;
muy     = VehiclePlant.muy;
nF      = VehiclePlant.nF;
nR      = VehiclePlant.nR;
alpha   = 0:0.5:15;
alpha   = alpha*pi/180;
FyLin   = - TireModel.Characteristic(alpha);
FyFPac  = - TirePlant.Characteristic(alpha, FzF, muy);
FyRPac  = - TirePlant.Characteristic(alpha, FzR, muy);
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(alpha(1:floor(end/2))*180/pi,FyLin(1:floor(end/2)),'r')
plot(alpha*180/pi,FyFPac,'g')
plot(alpha*180/pi,FyRPac,'g--')
xlabel('alpha [deg]')
ylabel('Fy [N]')
l = legend('Linear','Pacejka F','Pacejka R');
set(l,'Location','SouthEast')
%%
% Comparando os estados
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XTPlant,'r')
plot(TSPAN,XTModel,'r--')
xlabel('Time [s]')
ylabel('Distance X [m]')
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,YTPlant,'g')
plot(TSPAN,YTModel,'g--')
xlabel('Time [s]')
ylabel('Distance Y [m]')
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,PSIPlant,'b')
plot(TSPAN,PSIModel,'b--')
xlabel('Time [s]')
ylabel('PSI [rad]')
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,vTPlant,'c')
plot(TSPAN,vTModel,'c--')
xlabel('Time [s]')
ylabel('vT [m/s]')
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,ALPHATPlant,'m'),
plot(TSPAN,ALPHATModel,'m--'),
xlabel('Time [s]')
ylabel('ALPHAT [rad/s]')
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,dPSIPlant,'k')
plot(TSPAN,dPSIModel,'k--')
xlabel('Time [s]')
ylabel('dPSI [rad/s]')
%%
% Comparando a acelera玢o longitudinal e transversal
saidasPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];
matDerivEstadosPlant = zeros(size(saidasPlant));
for i = 1:size(saidasPlant,1)
    auxil = simulatorPlant.Vehicle.Model(1,saidasPlant(i,:),TSPAN);
    matDerivEstadosPlant(i,:) = auxil';
end
dXTPlant = matDerivEstadosPlant(:,1);
dYTPlant = matDerivEstadosPlant(:,2);
dPSIPlant = matDerivEstadosPlant(:,3);
dvTPlant = matDerivEstadosPlant(:,4);
dALPHATPlant = matDerivEstadosPlant(:,5);
ddPSIPlant = matDerivEstadosPlant(:,6);
ddXPlant = dvTPlant.*cos(PSIPlant + ALPHATPlant) - vTPlant.*(dPSIPlant + dALPHATPlant).*sin(PSIPlant + ALPHATPlant);
ddYPlant = dvTPlant.*sin(PSIPlant + ALPHATPlant) + vTPlant.*(dPSIPlant + dALPHATPlant).*cos(PSIPlant + ALPHATPlant);
ACELNumPlant = [(ddXPlant.*cos(PSIPlant) - ddYPlant.*sin(PSIPlant))  (-ddXPlant.*sin(PSIPlant) + ddYPlant.*cos(PSIPlant))];
saidasModel = [XTModel YTModel PSIModel vTModel ALPHATModel dPSIModel];
matDerivEstadosModel = zeros(size(saidasModel));
for i = 1:size(saidasModel,1)
    auxil = simulatorModel.Vehicle.Model(1,saidasModel(i,:),TSPAN);
    matDerivEstadosModel(i,:) = auxil';
end
dXTModel = matDerivEstadosModel(:,1);
dYTModel = matDerivEstadosModel(:,2);
dPSIModel = matDerivEstadosModel(:,3);
dvTModel = matDerivEstadosModel(:,4);
dALPHATModel = matDerivEstadosModel(:,5);
ddPSIModel = matDerivEstadosModel(:,6);
ddXModel = dvTModel.*cos(PSIModel + ALPHATModel) - vTModel.*(dPSIModel + dALPHATModel).*sin(PSIModel + ALPHATModel);
ddYModel = dvTModel.*sin(PSIModel + ALPHATModel) + vTModel.*(dPSIModel + dALPHATModel).*cos(PSIModel + ALPHATModel);
ACELNumModel = [(ddXModel.*cos(PSIModel) - ddYModel.*sin(PSIModel))  (-ddXModel.*sin(PSIModel) + ddYModel.*cos(PSIModel))];
figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,ACELNumPlant(:,1),'r')
plot(TSPAN,ACELNumPlant(:,2),'g')
plot(TSPAN,ACELNumModel(:,1),'r--')
plot(TSPAN,ACELNumModel(:,2),'g--')
xlabel('time [s]')
ylabel('acc. [m/s]')
l = legend('AX Plant','AY Plant','AX Model','AY Model');
set(l,'Location','NorthEast')
%%