mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 17:22:45 +00:00
Clean-Up & Compute Linearized Model
- Delete nonlinear optimization functions - Move intermediate kinematic model computations to `bike_model_helper` function - Compute discrete-time linearized model in `linearized_bike_model` function
This commit is contained in:
Sravan Balaji
@@ -287,184 +287,94 @@ function dzdt=kinematic_bike(t,x,U,T)
|
|||||||
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
|
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
|
||||||
end
|
end
|
||||||
|
|
||||||
%% nonlinear opt functions
|
%% Kinematic Bike Models
|
||||||
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)
|
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
|
global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims
|
||||||
|
|
||||||
|
[x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(X,U);
|
||||||
|
|
||||||
|
% 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] = linearized_bike_model(X_ref,U_ref)
|
||||||
|
global Nw f Iz a b By Cy Dy Ey Shy Svy m g
|
||||||
|
|
||||||
|
syms x_var u_var y_var v_var psi_var r_var ... % states
|
||||||
|
delta_f_var F_x_var ... % inputs
|
||||||
|
F_yf_var F_yr_var % lateral forces
|
||||||
|
|
||||||
|
% Vehicle Dynamics (Equation 1)
|
||||||
|
dx = u_var*cos(psi_var) - v_var*sin(psi_var);
|
||||||
|
du = (1/m)*(-f*m*g + Nw*F_x_var - F_yf_var*sin(delta_f_var)) + v_var*r_var;
|
||||||
|
dy = u_var*sin(psi_var) + v_var*cos(psi_var);
|
||||||
|
dv = (1/m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var;
|
||||||
|
dpsi = r_var;
|
||||||
|
dr = (1/Iz)*(a*F_yf_var*cos(delta_f_var) - b*F_yr_var);
|
||||||
|
|
||||||
|
% Jacobians of continuous-time linearized system
|
||||||
|
% TODO: Change A_c & B_c computation to regular functions (e.g.,
|
||||||
|
% using `matlabFunction`)
|
||||||
|
A_c_symb = [ ...
|
||||||
|
diff(dx, x_var), diff(dx, u_var), diff(dx, y_var), diff(dx, v_var), diff(dx, psi_var), diff(dx, r_var);
|
||||||
|
diff(du, x_var), diff(du, u_var), diff(du, y_var), diff(du, v_var), diff(du, psi_var), diff(du, r_var);
|
||||||
|
diff(dy, x_var), diff(dy, u_var), diff(dy, y_var), diff(dy, v_var), diff(dy, psi_var), diff(dy, r_var);
|
||||||
|
diff(dv, x_var), diff(dv, u_var), diff(dv, y_var), diff(dv, v_var), diff(dv, psi_var), diff(dv, r_var);
|
||||||
|
diff(dpsi,x_var), diff(dpsi,u_var), diff(dpsi,y_var), diff(dpsi,v_var), diff(dpsi,psi_var), diff(dpsi,r_var);
|
||||||
|
diff(dr, x_var), diff(dr, u_var), diff(dr, y_var), diff(dr, v_var), diff(dr, psi_var), diff(dr, r_var);
|
||||||
|
];
|
||||||
|
|
||||||
|
B_c_symb = [ ...
|
||||||
|
diff(dx, delta_f_var), diff(dx, F_x_var);
|
||||||
|
diff(du, delta_f_var), diff(du, F_x_var);
|
||||||
|
diff(dy, delta_f_var), diff(dy, F_x_var);
|
||||||
|
diff(dv, delta_f_var), diff(dv, F_x_var);
|
||||||
|
diff(dpsi,delta_f_var), diff(dpsi,F_x_var);
|
||||||
|
diff(dr, delta_f_var), diff(dr, F_x_var);
|
||||||
|
];
|
||||||
|
|
||||||
|
% Substitute values from reference trajectory into symbolic Jacobians
|
||||||
|
% TODO: Fix reference trajectory indexing with "i"
|
||||||
|
A_c = @(i) double( ...
|
||||||
|
subs( ...
|
||||||
|
A_c_symb, ...
|
||||||
|
[x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
|
||||||
|
bike_model_helper(X_ref(i), U_ref(i)) ...
|
||||||
|
) ...
|
||||||
|
);
|
||||||
|
B_c = @(i) double( ...
|
||||||
|
subs( ...
|
||||||
|
B_c_symb, ...
|
||||||
|
[x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
|
||||||
|
bike_model_helper(X_ref(i), U_ref(i)) ...
|
||||||
|
) ...
|
||||||
|
);
|
||||||
|
|
||||||
|
% Discrete-time LTV system
|
||||||
|
A = @(i) eye(6) + dt*A_c(i);
|
||||||
|
B = @(i) dt * B_c(i);
|
||||||
|
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
|
||||||
|
|
||||||
|
function [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(X,U)
|
||||||
|
global Nw f Iz a b By Cy Dy Ey Shy Svy m g
|
||||||
|
|
||||||
% Get state & input variables
|
% Get state & input variables
|
||||||
x = X(1);
|
x = X(1);
|
||||||
u = X(2);
|
u = X(2);
|
||||||
@@ -507,74 +417,4 @@ function dXdt = nonlinear_bike_model(X,U)
|
|||||||
% Apply input limits (Table 1)
|
% Apply input limits (Table 1)
|
||||||
delta_f = clamp(delta_f, delta_lims(1), delta_lims(2));
|
delta_f = clamp(delta_f, delta_lims(1), delta_lims(2));
|
||||||
F_x = clamp(F_x, Fx_lims(1), Fx_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
|
end
|
||||||
Reference in New Issue
Block a user