% main.m - This mfile is the main file. It sets up the desired yaw
% rate trajectory and initializes the constant vehicle parameters and
% linear vehicle model used by the controller. It also calculates the
% LQR gains and sets up the desired failure case. The simulink
% simulation file car4.mdl is then run one time for each velocity
% specified in the vector 'vel' and for each quadratic weight on the
% virtual effector specified in the vector 'wBeta'.
clear all
close all
clc
% set up desired yaw profile===========================================
lanes = 2; %how many lanes to change(setup desired position vector)
psi_des(1) = 0;%lane change ~4m = 157.5in
rdes(1) = 0;
tt = 0;
dt = 0.01; %time step
in = 0;
for ii = 1:1200
if ii<396
psi_des(ii) = 0;
rdes(ii) = 0;
pos_des(ii) = 0;
end
if ((ii>395) & (ii<788))
psi_des(ii) = 0.075*(1-cos(1.60*tt(ii-1))); % desired heading
rdes(ii) = (psi_des(ii) - psi_des(ii-1))/dt; % desired yaw rate
pos_des(ii) = in+(psi_des(ii-1)+psi_des(ii))*dt/2;
in = pos_des(ii);
end
if ii>=788
psi_des(ii) = 0;
rdes(ii) = 0;
pos_des(ii) = in;
end
tt(ii) = dt*(ii-1);
end
r_des = [tt 'rdes'];
psi_des = [tt 'psi_des'];
pos_des = [tt 'lanes*13.58*pos_des'];
% Project parameters ==================================================
g = 9.81; % gravity (m/s^2)
lb_N = 4.448; % converts lbs to N
rad_d = 180/pi; % converts radians to degrees
% car parameters %
L = 2.715; % wheelbase of car (m)
T_f = 1.554; % track width of front axle (m)
T_r = 1.534; % trach width of rear axle (m)
W = 13735.424; % curb weight of car + 200lb driver(N)
a = 1.013; % distance from front wheel to cg (m)
b = L-a; % distance from rear wheel to cg (m)
Wf = W*b/L; % weight of front 1/2 car (N)
Wr = W*a/L; % weight of rear 1/2 car (N)
mass = W/g; % mass of car (kg)
Ixx =(0.18*3088-150) *32.174*0.04214011; %roll moment of inertia
Iyy =(0.99*3088-1149)*32.174*0.04214011; %pitch moment of inertia
Izz =(1.03*3088-1206)*32.174*0.04214011; %yaw moment of inertia(kg*m^2)
hcg = 0.4*1.4554; %height of center of gravity(m)(40%roof height)
hf = 0.127; hr = 0.127; % height of roll center in front & rear (m)
del_h = hr-hf; % difference in roll center heights
if del_h == 0 % calc distance between cg and roll axis
h1 = hcg-hf;
else
h2 = a*del_h/(a+b);
h1 = hcg-hf-h2;
end
Ca_f = 1975*180/pi; % tire stiffness x2 front axl (N/rad)
Ca_r = 1575*180/pi; % tire stiffness x2 rear axl (N/rad)
% Roll stiffness for Front and Rear %
Kphf = 750*180/pi; % total stiffness fr axl (N*m/rad)
Kphr = 650*180/pi; % total stiffness rr axl (N*m/rad)
Kus=Wf/(Ca_f*pi/180)-Wr/(Ca_r*pi/180) % understeer gradient
count=1;
vel=[45, 55, 65]; % simulate at these velocities (mph)
for ii=1:length(vel)
% Linear Car Model ================================================
V = vel(ii)*0.44704; % mi/hr x0.44704 =(m/s)
C0 = Ca_f + Ca_r;
C1 = a*Ca_f - b*Ca_r;
C2 = a^2 * Ca_f + b^2 * Ca_r;
A = [-C0/(mass*V), -C1/(mass*V^2)-1 ; %[beta]
-C1/Izz, -C2/(V*Izz) ]; %[ r ]
B = [Ca_f/(mass*V), Ca_r/(mass*V), 0, 0, 1;%[delt_f] %[delt_r]
a*Ca_f/Izz, -b*Ca_r/Izz, T_f/(2*Izz), T_r/(2*Izz), C1*mass*V/(Izz*C0)]; %[delFxf]
%[delFxr]
%[virEff]
C = [1 0; 0 1];
D = zeros(size(B));
[mm,nn]=size(B); % Quad_Prog s-function parameters
% Controller Gains ================================================
[Ad, Bd] = c2d(A,B,0.01); % discrete B is used by QP (Bd*u = ubar)
% add integrators to sys to minimize error
Ap = [A, zeros(2,1);0, 1, 0];
Bp = [1 0; 0 1; 0 0];
[Ad, Bpd] = c2d(Ap,Bp,0.01); %gains computed assuming perfect input
[K,S,E] = dlqr(Ad,Bpd,0.5*eye(3),eye(2)); % compute LQR gains
algName = {'QP'};%, 'SP'};%, 'LS'};
algFlg=[1];
for jj=1:length(algFlg)
% Run Simulation ==============================================
mu = 0.8; %approx coef of static friction between tire and road
algorithm = algFlg(jj); % 1: QP, 3: SPQP, 0: LS
wBeta = [1e3 1e7]; %quadratic weight on "virtual-effector"
Beta = [3 7];
% implement failures
fault1 = diag([1, 1, 1, 1, 1]); % condition before failure
thresh = 5.25; % time of failure
fault2 = diag([0, 1, 1, 1, 1]); % condition after failure
FailName = 'Fsteer'; % name of the failure
for kk = 1:length(wBeta)
wB = wBeta(kk);
% save all simulation data
cmd = sprintf('save results\\%s_%s_B%i_%i.mat yawRateR sideslipR rollRateR rollAngR yawErrR yawDes posDes Cmds psiDes X Y heading sigma V algorithm Bd fLim rLim ubar', FailName, algName{jj}, Beta(kk), vel(ii));
fprintf('running simulation %i of %i ...', count, ...
length(vel)*length(algFlg)*length(wBeta));
sim('car4',[3, 10]); % run the simulation
fprintf('done\n');
eval(cmd);
count=count+1;
end
end
end
analysis(vel,algName,Beta,FailName); % function to analyze sim results
% plots; % generate plots
