mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 09:12:45 +00:00
Start Implementing MPC based on Xenia's work
- Copy `xenia_nonlinearopt.m` to `xenia_sravan_nonlinearopt.m` - Clean up nonlinear bike model: add comments, whitespace, intermediate variables, etc. - Add helper function `clamp` to clamp a value between min and max limits
This commit is contained in:
Sravan Balaji
580
Experimentation/xenia_sravan_nonlinearopt.m
Normal file
580
Experimentation/xenia_sravan_nonlinearopt.m
Normal file
@@ -0,0 +1,580 @@
|
|||||||
|
%% System Parameters
|
||||||
|
% Vehicle Parameters (Table 1)
|
||||||
|
global Nw f Iz a b By Cy Dy Ey Shy Svy m g
|
||||||
|
Nw = 2;
|
||||||
|
f = 0.01;
|
||||||
|
Iz = 2667;
|
||||||
|
a = 1.35;
|
||||||
|
b = 1.45;
|
||||||
|
By = 0.27;
|
||||||
|
Cy = 1.2;
|
||||||
|
Dy = 0.7;
|
||||||
|
Ey = -1.6;
|
||||||
|
Shy = 0;
|
||||||
|
Svy = 0;
|
||||||
|
m = 1400;
|
||||||
|
g = 9.806;
|
||||||
|
|
||||||
|
% Input Limits (Table 1)
|
||||||
|
global delta_lims Fx_lims
|
||||||
|
delta_lims = [-0.5, 0.5];
|
||||||
|
Fx_lims = [-5e3, 5e3];
|
||||||
|
|
||||||
|
% Initial Conditions (Equation 15)
|
||||||
|
state_init = [ ...
|
||||||
|
287; ... % x [m]
|
||||||
|
5; ... % u [m/s]
|
||||||
|
-176; ... % y [m]
|
||||||
|
0; ... % v [m/s]
|
||||||
|
2; ... % psi [rad]
|
||||||
|
0; ... % r [rad/s]
|
||||||
|
];
|
||||||
|
|
||||||
|
%% Load Files
|
||||||
|
load("TestTrack.mat");
|
||||||
|
|
||||||
|
%% Setup
|
||||||
|
curr_pos = [state_init(1);state_init(3)];
|
||||||
|
|
||||||
|
%state_size = tbd;
|
||||||
|
% z = [init, init, -0.004, 3900]; %for testing purposes
|
||||||
|
% nsteps = 2;
|
||||||
|
% [LB, UB] = bounds(nsteps, 1);
|
||||||
|
% [g,h,dg,dh]=nonlcon(z, Xobs, nsteps);
|
||||||
|
% [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps);
|
||||||
|
|
||||||
|
|
||||||
|
Nobs = randi([10 25], 1,1);
|
||||||
|
global Xobs
|
||||||
|
Xobs = generateRandomObstacles(Nobs);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
U_final = [];
|
||||||
|
Y_final = [];
|
||||||
|
options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
|
||||||
|
'SpecifyObjectiveGradient',true) ;
|
||||||
|
|
||||||
|
load('ROB535_ControlProject_part1_Team3.mat');
|
||||||
|
|
||||||
|
%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init);
|
||||||
|
load('reftrack_info.mat');
|
||||||
|
|
||||||
|
load('segments_info.mat');
|
||||||
|
|
||||||
|
%% MPC
|
||||||
|
U_ref = ROB535_ControlProject_part1_input';
|
||||||
|
Y_ref = Y_submission';
|
||||||
|
dt = 0.01;
|
||||||
|
|
||||||
|
%discretize dynamics
|
||||||
|
F_zf=b/(a+b)*m*g;
|
||||||
|
F_zr=a/(a+b)*m*g;
|
||||||
|
|
||||||
|
%we are just going to use cornering stiffness to make linear so this derivative
|
||||||
|
%easier, the vehicle parameter's are close enough to problem 1 hw 2
|
||||||
|
B=10;
|
||||||
|
C=1.3;
|
||||||
|
D=1;
|
||||||
|
Ca_r= F_zr*B*C*D;
|
||||||
|
Ca_f= F_zf*B*C*D;
|
||||||
|
|
||||||
|
x = @(s,i) Y_ref(s,i);
|
||||||
|
|
||||||
|
Ac = @(i) [0, cos(x(5,i)), 0, -sin(x(5,i)), x(2,i)*sin(x(5,i))-x(4,i)*cos(x(5,i)), 0;
|
||||||
|
0, (-1/m)*Ca_f*x(2,i)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
|
||||||
|
0, sin(x(5,i)), 0, cos(x(5,i)), -x(4,i)*sin(x(5,i))+x(2,i)*cos(x(5,i)), 0;
|
||||||
|
0, (1/m)*(-Ca_f*x(2,i)^-2 - Ca_r*x(2,i)^-2) - 1, 0, Ca_r/m*(-1/x(2,i)) + Ca_f/m*(-1/x(2,i)), 0, Ca_r/m*(b/x(2,i)) + Ca_f/m*(-a/x(2,i)) - x(2,i);
|
||||||
|
0, 0, 0, 0, 0, 1
|
||||||
|
0, (1/Iz)*(-Ca_f*a*x(2,i)^-2 - b*Ca_r*x(2,i)^-2), 0, -b*Ca_r/Iz*(-1/x(2,i)) + a*Ca_f/Iz*(-1/x(2,i)), 0, -b*Ca_r/Iz*(b/x(2,i)) + a*Ca_f/Iz*(-a/x(2,i))];
|
||||||
|
|
||||||
|
Bc = @(i) [0, -Ca_f/m, 0, Ca_f/m, 0, a*Ca_f/Iz;
|
||||||
|
0, Nw/m, 0, 0, 0, 0]';
|
||||||
|
|
||||||
|
A = @(i) eye(6) + dt*Ac(i);
|
||||||
|
B = @(i) dt*Bc(i);
|
||||||
|
|
||||||
|
|
||||||
|
%Decision variables
|
||||||
|
npred = 10;
|
||||||
|
nstates = 6;
|
||||||
|
ninputs = 2;
|
||||||
|
Ndec=(npred+1)*nstates+ninputs*npred;
|
||||||
|
|
||||||
|
input_range = [-0.5, 0.5; -5000, 5000];
|
||||||
|
eY0 = state_init';
|
||||||
|
[Aeq_test1,beq_test1]=eq_cons(1,A,B,eY0,npred,nstates,ninputs);
|
||||||
|
%simulate forward
|
||||||
|
T = 0:0.01:(size(Y_ref,2)/100);
|
||||||
|
%final trajectory
|
||||||
|
Y=NaN(nstates,length(T));
|
||||||
|
|
||||||
|
%applied inputs
|
||||||
|
U=NaN(ninputs,length(T));
|
||||||
|
|
||||||
|
%input from QP
|
||||||
|
u_mpc=NaN(ninputs,length(T));
|
||||||
|
|
||||||
|
%error in states (actual-reference)
|
||||||
|
eY=NaN(nstates,length(T));
|
||||||
|
|
||||||
|
%set random initial condition
|
||||||
|
Y(:,1)=eY0+Y_ref(:,1);
|
||||||
|
|
||||||
|
for i=1:length(T)-1
|
||||||
|
i
|
||||||
|
%shorten prediction horizon if we are at the end of trajectory
|
||||||
|
npred_i=min([npred,length(T)-i]);
|
||||||
|
|
||||||
|
%calculate error
|
||||||
|
eY(:,i)=Y(:,i)-Y_ref(:,i);
|
||||||
|
|
||||||
|
%generate equality constraints
|
||||||
|
[Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs);
|
||||||
|
|
||||||
|
%generate boundary constraints
|
||||||
|
[Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs);
|
||||||
|
|
||||||
|
%cost for states
|
||||||
|
Q=[1,1,1,1,1,1];
|
||||||
|
|
||||||
|
%cost for inputs
|
||||||
|
R=[0.1,0.01];
|
||||||
|
|
||||||
|
H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);
|
||||||
|
|
||||||
|
f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1);
|
||||||
|
|
||||||
|
[x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub);
|
||||||
|
|
||||||
|
%get linearized input
|
||||||
|
u_mpc(:,i)=x(nstates*(npred_i+1)+1:nstates*(npred_i+1)+ninputs);
|
||||||
|
|
||||||
|
%get input
|
||||||
|
U(:,i)=u_mpc(:,i)+U_ref(:,i);
|
||||||
|
|
||||||
|
|
||||||
|
%simulate model
|
||||||
|
[~,ztemp]=ode45(@(t,z)kinematic_bike(t,z,U(:,i),0),[0 dt],Y(:,i));
|
||||||
|
|
||||||
|
%store final state
|
||||||
|
Y(:,i+1)=ztemp(end,:)';
|
||||||
|
end
|
||||||
|
|
||||||
|
|
||||||
|
%% function start
|
||||||
|
|
||||||
|
function [start_idx, end_idx] = get_indices(segment_num, num_pts)
|
||||||
|
if segment_num == 1
|
||||||
|
start_idx = 1;
|
||||||
|
end_idx = num_pts(segment_num);
|
||||||
|
else
|
||||||
|
start_idx = sum(num_pts(1:segment_num-1)) + 1;
|
||||||
|
end_idx = sum(num_pts(1:segment_num));
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
%not used
|
||||||
|
function x0vec = initvec(x0, u0)
|
||||||
|
%function used because fmincon needs initial condition to be size of
|
||||||
|
%state vector
|
||||||
|
%x0 - last row of Y at checkpoint
|
||||||
|
%u0 - last row of U at checkpoint
|
||||||
|
global nsteps
|
||||||
|
x0vec = [];
|
||||||
|
for i = 1:nsteps
|
||||||
|
x0vec = [x0vec, x0];
|
||||||
|
end
|
||||||
|
|
||||||
|
%not sure if inputs should be instantiated or not
|
||||||
|
%will instantiate them to previous u
|
||||||
|
for i = 1:nsteps-1
|
||||||
|
x0vec = [x0vec, u0];
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
%% mpc functions
|
||||||
|
function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
|
||||||
|
%build matrix for A_i*x_i+B_i*u_i-x_{i+1}=0
|
||||||
|
%in the form Aeq*z=beq
|
||||||
|
%initial_idx specifies the time index of initial condition from the reference trajectory
|
||||||
|
%A and B are function handles above
|
||||||
|
|
||||||
|
%initial condition
|
||||||
|
x_initial=x_initial(:);
|
||||||
|
|
||||||
|
%size of decision variable and size of part holding states
|
||||||
|
zsize=(npred+1)*nstates+npred*ninputs;
|
||||||
|
xsize=(npred+1)*nstates;
|
||||||
|
|
||||||
|
Aeq=zeros(xsize,zsize);
|
||||||
|
Aeq(1:nstates,1:nstates)=eye(nstates); %initial condition
|
||||||
|
beq=zeros(xsize,1);
|
||||||
|
beq(1:nstates)=x_initial;
|
||||||
|
|
||||||
|
state_idxs=nstates+1:nstates:xsize;
|
||||||
|
input_idxs=xsize+1:ninputs:zsize;
|
||||||
|
|
||||||
|
for i=1:npred
|
||||||
|
%negative identity for i+1
|
||||||
|
Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i):state_idxs(i)+nstates-1)=-eye(nstates);
|
||||||
|
|
||||||
|
%A matrix for i
|
||||||
|
Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i)-nstates:state_idxs(i)-1)=A(initial_idx+i-1);
|
||||||
|
|
||||||
|
%B matrix for i
|
||||||
|
Aeq(state_idxs(i):state_idxs(i)+nstates-1,input_idxs(i):input_idxs(i)+ninputs-1)=B(initial_idx+i-1);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs)
|
||||||
|
%time_idx is the index along uref the initial condition is at
|
||||||
|
xsize=(npred+1)*nstates;
|
||||||
|
usize=npred*ninputs;
|
||||||
|
|
||||||
|
Lb=[];
|
||||||
|
Ub=[];
|
||||||
|
for j=1:ninputs
|
||||||
|
Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
|
||||||
|
Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
|
||||||
|
end
|
||||||
|
|
||||||
|
Lb=reshape(Lb,[usize,1]);
|
||||||
|
Ub=reshape(Ub,[usize,1]);
|
||||||
|
|
||||||
|
Lb=[-Inf(xsize,1);Lb];
|
||||||
|
Ub=[Inf(xsize,1);Ub];
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
|
function dzdt=kinematic_bike(t,x,U,T)
|
||||||
|
global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims
|
||||||
|
|
||||||
|
%generate input functions
|
||||||
|
delta_f=U(1);
|
||||||
|
F_x=U(2);
|
||||||
|
|
||||||
|
%slip angle functions in degrees
|
||||||
|
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
|
||||||
|
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
|
||||||
|
|
||||||
|
%Nonlinear Tire Dynamics
|
||||||
|
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
|
||||||
|
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
|
||||||
|
|
||||||
|
F_zf=b/(a+b)*m*g;
|
||||||
|
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
|
||||||
|
|
||||||
|
F_zr=a/(a+b)*m*g;
|
||||||
|
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
|
||||||
|
|
||||||
|
F_total=sqrt((Nw*F_x)^2+(F_yr^2));
|
||||||
|
F_max=0.7*m*g;
|
||||||
|
|
||||||
|
if F_total>F_max
|
||||||
|
|
||||||
|
F_x=F_max/F_total*F_x;
|
||||||
|
|
||||||
|
F_yr=F_max/F_total*F_yr;
|
||||||
|
end
|
||||||
|
|
||||||
|
%vehicle dynamics
|
||||||
|
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
|
||||||
|
(-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
|
||||||
|
x(2)*sin(x(5))+x(4)*cos(x(5));...
|
||||||
|
(F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
|
||||||
|
x(6);...
|
||||||
|
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
|
||||||
|
end
|
||||||
|
|
||||||
|
%% nonlinear opt functions
|
||||||
|
function [lb, ub] = bounds(start_idx, end_idx)
|
||||||
|
load('TestTrack.mat');
|
||||||
|
load('reftrack_info.mat');
|
||||||
|
|
||||||
|
%[m,nPts] = size(TestTrack.cline);
|
||||||
|
% numState = 6;
|
||||||
|
% numInput = 2;
|
||||||
|
% nsteps = StepsPerPoint * nPts;
|
||||||
|
|
||||||
|
lb_u = [-0.5;-5000];
|
||||||
|
ub_u = [0.5;5000];
|
||||||
|
|
||||||
|
bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ...
|
||||||
|
TestTrack.br(1,1), TestTrack.br(1,2)];
|
||||||
|
bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ...
|
||||||
|
TestTrack.br(2,1), TestTrack.br(2,2)];
|
||||||
|
phi_init = TestTrack.theta(1);
|
||||||
|
|
||||||
|
%phi restricted to just [-pi pi]
|
||||||
|
%lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
|
||||||
|
%ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
|
||||||
|
lb = []; ub = [];
|
||||||
|
|
||||||
|
%hijacking this for loop to only consider one index for bounds
|
||||||
|
for i=start_idx:end_idx+1
|
||||||
|
Y_ref_curxy = [Y_submission(i,1); Y_submission(i,3)];
|
||||||
|
sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2;
|
||||||
|
dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
|
||||||
|
[minDist, indMin] = min(dist_to_cl);
|
||||||
|
|
||||||
|
prev_idx = max(indMin-1, 1);
|
||||||
|
next_idx = min(indMin, 246);
|
||||||
|
|
||||||
|
bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
|
||||||
|
TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
|
||||||
|
bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
|
||||||
|
TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
|
||||||
|
%phi_init = TestTrack.theta(i);
|
||||||
|
|
||||||
|
lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
|
||||||
|
ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
|
||||||
|
|
||||||
|
|
||||||
|
lb=[lb;lb_x];
|
||||||
|
ub=[ub;ub_x];
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
|
for i=start_idx:end_idx
|
||||||
|
ub=[ub;ub_u];
|
||||||
|
lb=[lb;lb_u];
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
function [J, dJ] = costfun(z)
|
||||||
|
global nsteps
|
||||||
|
global target_vec
|
||||||
|
load('TestTrack.mat');
|
||||||
|
|
||||||
|
targetPos = target_vec(1:2);
|
||||||
|
targetTheta = target_vec(3);
|
||||||
|
sumPos = [];
|
||||||
|
sumInput = [];
|
||||||
|
for i = 1:nsteps
|
||||||
|
zIndx = 6*(i-1) + 1;
|
||||||
|
sumPos(i) = (z(zIndx) - targetPos(1))^2 + (z(zIndx+2) - targetPos(2))^2 + (z(zIndx+4) - targetTheta)^2;
|
||||||
|
|
||||||
|
if (i <= nsteps-1)
|
||||||
|
uInd = 2*(i-1) + nsteps*6 - 1;
|
||||||
|
sumInput(i) = z(uInd)^2 + z(uInd+1)^2;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
J = sum(sumPos) + sum(sumInput);
|
||||||
|
dJ = transpose(z.*2);
|
||||||
|
|
||||||
|
uStart = nsteps*6 - 1;
|
||||||
|
for i = 1:6:nsteps*6
|
||||||
|
zIndx = i;
|
||||||
|
dJ(i) = (z(zIndx) - targetPos(1))*2;
|
||||||
|
dJ(i+2) = (z(zIndx+2) - targetPos(2))*2;
|
||||||
|
dJ(i+4) = (z(zIndx+4) - targetTheta)*2;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
function [g, h, dg, dh] = nonlcon(z)
|
||||||
|
%nsteps = (size(z,2)/8);
|
||||||
|
global nsteps
|
||||||
|
global Xobs
|
||||||
|
curr_pos = [z(1); z(3)];
|
||||||
|
|
||||||
|
Xobs_seen = senseObstacles(curr_pos, Xobs);
|
||||||
|
centroids = [];
|
||||||
|
for i = 1:size(Xobs_seen,2)
|
||||||
|
centroids = [centroids; mean(Xobs_seen{1, i})];
|
||||||
|
end
|
||||||
|
|
||||||
|
dt = 0.01;
|
||||||
|
g = []; dg = [];
|
||||||
|
|
||||||
|
%radius size
|
||||||
|
r = 3;
|
||||||
|
|
||||||
|
for i = 1:nsteps
|
||||||
|
zInd_x = 6*(i-1) + 1;
|
||||||
|
zInd_y = 6*(i-1) + 3;
|
||||||
|
curr_xy = [z(zInd_x), z(zInd_y)];
|
||||||
|
|
||||||
|
%g based on if there's obstacles around
|
||||||
|
%initialize to zero
|
||||||
|
g_curr = 0;
|
||||||
|
zeroRow = zeros(1,size(z,2));
|
||||||
|
dg(i,:) = zeroRow;
|
||||||
|
if (~isempty(centroids))
|
||||||
|
dist2Obst = [];
|
||||||
|
for j = 1:size(Xobs_seen,2)
|
||||||
|
dist = norm(curr_xy(1) - centroids(j,1)) + norm(curr_xy(2) - centroids(j,2));
|
||||||
|
dist2Obst = [dist2Obst; dist];
|
||||||
|
end
|
||||||
|
|
||||||
|
%closest obstacle used for constraint
|
||||||
|
[minDist, indMin] = min(dist2Obst);
|
||||||
|
|
||||||
|
%g
|
||||||
|
g_curr = r^2 - (curr_xy(1) - centroids(indMin, 1))^2 - (curr_xy(2) - centroids(indMin, 2))^2;
|
||||||
|
|
||||||
|
%dg
|
||||||
|
dg(i,zInd_x) = curr_xy(1)*(-2) - centroids(indMin, 1)*2;
|
||||||
|
dg(i,zInd_y) = curr_xy(2)*(-2) - centroids(indMin, 2)*2;
|
||||||
|
end
|
||||||
|
g = [g; g_curr];
|
||||||
|
end
|
||||||
|
dg = transpose(dg);
|
||||||
|
|
||||||
|
h = z(1:6);
|
||||||
|
for i = 2:nsteps
|
||||||
|
zInd_x = 6*(i-1) + 1;
|
||||||
|
zInd_x_prev = 6*(i-2) + 1;
|
||||||
|
|
||||||
|
stateCurr = z(zInd_x:zInd_x+5);
|
||||||
|
statePrev = z(zInd_x_prev:zInd_x_prev+5);
|
||||||
|
|
||||||
|
%get previous input
|
||||||
|
uInd_prev = 2*(i-1) + nsteps*6 - 1;
|
||||||
|
uPrev = [z(uInd_prev), z(uInd_prev + 1)];
|
||||||
|
|
||||||
|
derPrev= dt*bike(statePrev, uPrev);
|
||||||
|
|
||||||
|
currH = stateCurr - statePrev - derPrev;
|
||||||
|
h(6*(i-1)+1) = currH(1);
|
||||||
|
h(6*(i-1)+2) = currH(2);
|
||||||
|
h(6*(i-1)+3) = currH(3);
|
||||||
|
h(6*(i-1)+4) = currH(4);
|
||||||
|
h(6*(i-1)+5) = currH(5);
|
||||||
|
h(6*(i-1)+6) = currH(6);
|
||||||
|
end
|
||||||
|
|
||||||
|
dh = zeros(size(z,2), nsteps*6);
|
||||||
|
dh(1:6, 1:6) = eye(6);
|
||||||
|
for i = 1:nsteps-1
|
||||||
|
uInd = 2*(i-1) + nsteps*6 + 1;
|
||||||
|
uCurr = [z(uInd), z(uInd + 1)];
|
||||||
|
zInd_x = 6*(i-1) + 1;
|
||||||
|
stateCurr = z(zInd_x:zInd_x+5);
|
||||||
|
[A, B] = bikeLinearize(stateCurr, uCurr);
|
||||||
|
|
||||||
|
dh(6*i+1:6*i+6, 6*i+1:6*i+6) = eye(6);
|
||||||
|
dh(6*i-5:6*i, 6*i+1:6*i+6) = -eye(6) - dt*A;
|
||||||
|
dh(nsteps*6+2*i-1:nsteps*6+2*i, 6*i+1:6*i+6) = -dt*transpose(B);
|
||||||
|
end
|
||||||
|
|
||||||
|
end
|
||||||
|
|
||||||
|
%% Linearization Functions
|
||||||
|
function dXdt = nonlinear_bike_model(X,U)
|
||||||
|
global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims
|
||||||
|
|
||||||
|
% Get state & input variables
|
||||||
|
x = X(1);
|
||||||
|
u = X(2);
|
||||||
|
y = X(3);
|
||||||
|
v = X(4);
|
||||||
|
psi = X(5);
|
||||||
|
r = X(6);
|
||||||
|
|
||||||
|
delta_f = U(1);
|
||||||
|
F_x = U(2);
|
||||||
|
|
||||||
|
% Front and rear lateral slip angles in radians (Equations 8 & 9)
|
||||||
|
alpha_f_rad = delta_f - atan2(v + a*r, u);
|
||||||
|
alpha_r_rad = -atan2(v - b*r, u);
|
||||||
|
|
||||||
|
% Convert radians to degrees for other equations
|
||||||
|
alpha_f = rad2deg(alpha_f_rad);
|
||||||
|
alpha_r = rad2deg(alpha_r_rad);
|
||||||
|
|
||||||
|
% Nonlinear Tire Dynamics (Equations 6 & 7)
|
||||||
|
phi_yf = (1-Ey)*(alpha_f + Shy) + (Ey/By)*atan(By*(alpha_f + Shy));
|
||||||
|
phi_yr = (1-Ey)*(alpha_r + Shy) + (Ey/By)*atan(By*(alpha_r + Shy));
|
||||||
|
|
||||||
|
% Lateral forces using Pacejka "Magic Formula" (Equations 2 - 5)
|
||||||
|
F_zf = (b/(a+b))*(m*g);
|
||||||
|
F_yf = F_zf*Dy*sin(Cy*atan(By*phi_yf)) + Svy;
|
||||||
|
F_zr = (a/(a+b))*(m*g);
|
||||||
|
F_yr = F_zr*Dy*sin(Cy*atan(By*phi_yr)) + Svy;
|
||||||
|
|
||||||
|
% Limits on combined longitudinal and lateral loading of tires
|
||||||
|
% (Equations 10 - 14)
|
||||||
|
F_total = sqrt((Nw*F_x)^2 + (F_yr^2));
|
||||||
|
F_max = 0.7*(m*g);
|
||||||
|
|
||||||
|
if F_total > F_max
|
||||||
|
F_x = (F_max/F_total)*F_x;
|
||||||
|
F_yr = (F_max/F_total)*F_yr;
|
||||||
|
end
|
||||||
|
|
||||||
|
% Apply input limits (Table 1)
|
||||||
|
delta_f = clamp(delta_f, delta_lims(1), delta_lims(2));
|
||||||
|
F_x = clamp(F_x, Fx_lims(1), Fx_lims(2));
|
||||||
|
|
||||||
|
% Vehicle Dynamics (Equation 1)
|
||||||
|
dx = u*cos(psi) - v*sin(psi);
|
||||||
|
du = (1/m)*(-f*m*g + Nw*F_x - F_yf*sin(delta_f)) + v*r;
|
||||||
|
dy = u*sin(psi) + v*cos(psi);
|
||||||
|
dv = (1/m)*(F_yf*cos(delta_f) + F_yr) - u*r;
|
||||||
|
dpsi = r;
|
||||||
|
dr = (1/Iz)*(a*F_yf*cos(delta_f) - b*F_yr);
|
||||||
|
dXdt = [dx; du; dy; dv; dpsi; dr];
|
||||||
|
end
|
||||||
|
|
||||||
|
function [A, B] =bikeLinearize(x,U)
|
||||||
|
global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims
|
||||||
|
|
||||||
|
%generate input functions
|
||||||
|
delta_f= U(1);
|
||||||
|
F_x= U(2);
|
||||||
|
|
||||||
|
%slip angle functions in degrees
|
||||||
|
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
|
||||||
|
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
|
||||||
|
|
||||||
|
%Nonlinear Tire Dynamics
|
||||||
|
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
|
||||||
|
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
|
||||||
|
|
||||||
|
F_zf=b/(a+b)*m*g;
|
||||||
|
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
|
||||||
|
|
||||||
|
F_zr=a/(a+b)*m*g;
|
||||||
|
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
|
||||||
|
|
||||||
|
F_total=sqrt((Nw*F_x)^2+(F_yr^2));
|
||||||
|
F_max=0.7*m*g;
|
||||||
|
|
||||||
|
if F_total>F_max
|
||||||
|
|
||||||
|
F_x=F_max/F_total*F_x;
|
||||||
|
|
||||||
|
F_yr=F_max/F_total*F_yr;
|
||||||
|
end
|
||||||
|
|
||||||
|
%we are just going to use cornering stiffness to make linear so this derivative
|
||||||
|
%easier, the vehicle parameter's are close enough to problem 1 hw 2
|
||||||
|
B=10;
|
||||||
|
C=1.3;
|
||||||
|
D=1;
|
||||||
|
Ca_r= F_zr*B*C*D;
|
||||||
|
Ca_f= F_zf*B*C*D;
|
||||||
|
|
||||||
|
A = [0, cos(x(5)), 0, -sin(x(5)), x(2)*sin(x(5))-x(4)*cos(x(5)), 0;
|
||||||
|
0, (-1/m)*Ca_f*x(2)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
|
||||||
|
0, sin(x(5)), 0, cos(x(5)), -x(4)*sin(x(5))+x(2)*cos(x(5)), 0;
|
||||||
|
0, (1/m)*(-Ca_f*x(2)^-2 - Ca_r*x(2)^-2) - 1, 0, Ca_r/m*(-1/x(2)) + Ca_f/m*(-1/x(2)), 0, Ca_r/m*(b/x(2)) + Ca_f/m*(-a/x(2)) - x(2);
|
||||||
|
0, 0, 0, 0, 0, 1
|
||||||
|
0, (1/Iz)*(-Ca_f*a*x(2)^-2 - b*Ca_r*x(2)^-2), 0, -b*Ca_r/Iz*(-1/x(2)) + a*Ca_f/Iz*(-1/x(2)), 0, -b*Ca_r/Iz*(b/x(2)) + a*Ca_f/Iz*(-a/x(2))];
|
||||||
|
|
||||||
|
B = [0, -Ca_f/(x(2)*m), 0, Ca_f/m, 0, a*Ca_f/Iz;
|
||||||
|
0, Nw/m, 0, 0, 0, 0]';
|
||||||
|
end
|
||||||
|
|
||||||
|
%% Helper Functions
|
||||||
|
function clamped_val = clamp(val, min, max)
|
||||||
|
clamped_val = val;
|
||||||
|
|
||||||
|
if clamped_val < min
|
||||||
|
clamped_val = min;
|
||||||
|
elseif clamped_val > max
|
||||||
|
clamped_val = max;
|
||||||
|
end
|
||||||
|
end
|
||||||
Reference in New Issue
Block a user