mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 17:22: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: 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)]
|
% - Z_err: Decision Variable Error [Y_err(0);...;Y_err(n+1);U_err(0);...;U_err(n)]
|
||||||
|
|
||||||
|
%% Internal Variables
|
||||||
properties
|
properties
|
||||||
% Vehicle Parameters (Table 1)
|
% Vehicle Parameters (Table 1)
|
||||||
Nw = 2;
|
Nw = 2;
|
||||||
@@ -82,7 +83,7 @@ classdef MPC_Class
|
|||||||
];
|
];
|
||||||
end
|
end
|
||||||
|
|
||||||
% Public Functions
|
%% Public Functions
|
||||||
methods (Access = public)
|
methods (Access = public)
|
||||||
function obj = MPC_Class(TestTrack, Y_ref, U_ref)
|
function obj = MPC_Class(TestTrack, Y_ref, U_ref)
|
||||||
%MPC_CLASS Construct an instance of this class and
|
%MPC_CLASS Construct an instance of this class and
|
||||||
@@ -100,7 +101,7 @@ classdef MPC_Class
|
|||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
% Private Constraint Functions
|
%% Private Constraint Functions
|
||||||
methods (Access = private)
|
methods (Access = private)
|
||||||
function [Lb, Ub] = bound_cons(obj, ref_idx)
|
function [Lb, Ub] = bound_cons(obj, ref_idx)
|
||||||
%bound_cons Construct lower and upper bounds on states and inputs
|
%bound_cons Construct lower and upper bounds on states and inputs
|
||||||
@@ -144,7 +145,137 @@ classdef MPC_Class
|
|||||||
end
|
end
|
||||||
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)
|
methods (Access = private)
|
||||||
function idx = get_state_start_idx(obj, i)
|
function idx = get_state_start_idx(obj, i)
|
||||||
%get_state_start_idx Calculates starting index of state i in
|
%get_state_start_idx Calculates starting index of state i in
|
||||||
|
|||||||
Reference in New Issue
Block a user