diff --git a/Experimentation/MPC_Class.m b/Experimentation/MPC_Class.m index f645444..3e48aa8 100644 --- a/Experimentation/MPC_Class.m +++ b/Experimentation/MPC_Class.m @@ -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