mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 09:12:45 +00:00
Add Kinematic Models
- Add nonlinear and linearized bike model functions - Add `bike_model_helper` function to perform intermediate calculations
This commit is contained in:
Sravan Balaji
@@ -18,6 +18,7 @@ classdef MPC_Class
|
||||
% - Z: Decision Variable [Y(0);...;Y(n+1);U(0);...;U(n)]
|
||||
% - Z_err: Decision Variable Error [Y_err(0);...;Y_err(n+1);U_err(0);...;U_err(n)]
|
||||
|
||||
%% Internal Variables
|
||||
properties
|
||||
% Vehicle Parameters (Table 1)
|
||||
Nw = 2;
|
||||
@@ -82,7 +83,7 @@ classdef MPC_Class
|
||||
];
|
||||
end
|
||||
|
||||
% Public Functions
|
||||
%% Public Functions
|
||||
methods (Access = public)
|
||||
function obj = MPC_Class(TestTrack, Y_ref, U_ref)
|
||||
%MPC_CLASS Construct an instance of this class and
|
||||
@@ -100,7 +101,7 @@ classdef MPC_Class
|
||||
end
|
||||
end
|
||||
|
||||
% Private Constraint Functions
|
||||
%% Private Constraint Functions
|
||||
methods (Access = private)
|
||||
function [Lb, Ub] = bound_cons(obj, ref_idx)
|
||||
%bound_cons Construct lower and upper bounds on states and inputs
|
||||
@@ -144,7 +145,137 @@ classdef MPC_Class
|
||||
end
|
||||
end
|
||||
|
||||
% Private Helper Functions
|
||||
%% Private Kinematic Models
|
||||
methods (Access = private)
|
||||
function dYdt = nonlinear_bike_model(obj, Y, U)
|
||||
%nonlinear_bike_model Computes the derivative of the states
|
||||
% based on current states and inputs using the full
|
||||
% nonlinear bike model.
|
||||
|
||||
% NOTE: I don't think this function is currently being used at all
|
||||
|
||||
[~,u,~,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(Y, U);
|
||||
|
||||
% Vehicle Dynamics (Equation 1)
|
||||
dx = u*cos(psi) - v*sin(psi);
|
||||
du = (1/obj.m)*(-obj.f*obj.m*obj.g + obj.Nw*F_x - F_yf*sin(delta_f)) + v*r;
|
||||
dy = u*sin(psi) + v*cos(psi);
|
||||
dv = (1/obj.m)*(F_yf*cos(delta_f) + F_yr) - u*r;
|
||||
dpsi = r;
|
||||
dr = (1/obj.Iz)*(obj.a*F_yf*cos(delta_f) - obj.b*F_yr);
|
||||
dYdt = [dx; du; dy; dv; dpsi; dr];
|
||||
end
|
||||
|
||||
function [A, B] = linearized_bike_model(obj, ref_idx)
|
||||
%linearized_bike_model Computes the discrete-time LTV
|
||||
% system matrices of the nonlinear bike model linearized
|
||||
% around the reference trajectory starting at the index
|
||||
% closest to `curr_state`
|
||||
|
||||
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/obj.m)*(-obj.f*obj.m*obj.g + obj.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/obj.m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var;
|
||||
dpsi = r_var;
|
||||
dr = (1/obj.Iz)*(obj.a*F_yf_var*cos(delta_f_var) - obj.b*F_yr_var);
|
||||
|
||||
% Jacobians of continuous-time linearized system
|
||||
% TODO: Change A_c & B_c computation to regular functions (e.g.,
|
||||
% using `matlabFunction`) if speed is an issue
|
||||
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
|
||||
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(obj.Y_ref(ref_idx+i,:), obj.U_ref(ref_idx+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(obj.Y_ref(ref_idx+i,:), obj.U_ref(ref_idx+i,:)) ...
|
||||
) ...
|
||||
);
|
||||
|
||||
% Discrete-time LTV system
|
||||
A = @(i) eye(obj.nstates) + obj.T_s*A_c(i);
|
||||
B = @(i) obj.T_s * B_c(i);
|
||||
end
|
||||
|
||||
function [x,u,y,v,psi,r,delta_f,F_x,F_yf,F_yr] = bike_model_helper(obj, Y, U)
|
||||
%bike_model_helper Computes the intermediate values
|
||||
% and applies limits used by the kinematic bike
|
||||
% model before the final derivative
|
||||
|
||||
% Get state & input variables
|
||||
x = Y(1);
|
||||
u = Y(2);
|
||||
y = Y(3);
|
||||
v = Y(4);
|
||||
psi = Y(5);
|
||||
r = Y(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 + obj.a*r, u);
|
||||
alpha_r_rad = -atan2(v - obj.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-obj.Ey)*(alpha_f + obj.Shy) + (obj.Ey/obj.By)*atan(obj.By*(alpha_f + obj.Shy));
|
||||
phi_yr = (1-obj.Ey)*(alpha_r + obj.Shy) + (obj.Ey/obj.By)*atan(obj.By*(alpha_r + obj.Shy));
|
||||
|
||||
% Lateral forces using Pacejka "Magic Formula" (Equations 2 - 5)
|
||||
F_zf = (obj.b/(obj.a+obj.b))*(obj.m*obj.g);
|
||||
F_yf = F_zf*obj.Dy*sin(obj.Cy*atan(obj.By*phi_yf)) + obj.Svy;
|
||||
F_zr = (obj.a/(obj.a+obj.b))*(obj.m*obj.g);
|
||||
F_yr = F_zr*obj.Dy*sin(obj.Cy*atan(obj.By*phi_yr)) + obj.Svy;
|
||||
|
||||
% Limits on combined longitudinal and lateral loading of tires
|
||||
% (Equations 10 - 14)
|
||||
F_total = sqrt((obj.Nw*F_x)^2 + (F_yr^2));
|
||||
F_max = 0.7*(obj.m*obj.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, obj.delta_lims(1), obj.delta_lims(2));
|
||||
F_x = clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2));
|
||||
end
|
||||
end
|
||||
|
||||
%% Private Helper Functions
|
||||
methods (Access = private)
|
||||
function idx = get_state_start_idx(obj, i)
|
||||
%get_state_start_idx Calculates starting index of state i in
|
||||
|
||||
Reference in New Issue
Block a user