% Filename: MPC_Class.m % Written by: Sravan Balaji, Xenia Demenchuk, and Peter Pongsachai % Created: 10 Dec 2021 classdef MPC_Class %MPC_CLASS Provides 2 public functions: % 1. Constructor instantiates object with track % and reference trajectory information % 2. `compute_inputs` uses MPC to determine inputs % to vehicle that will track reference trajectory % while avoiding obstacles and staying on track % Miscellaneous Notes % - Error = Actual - Reference % - Y: State [x; u; y; v; psi; r] % - Y_err: State Error [x - x_ref; u - u_ref; y - y_ref; v - v_ref; psi - psi_ref; r - r_ref] % - U: Input [delta_f; F_x] % - U_err: Input Error [delta_f - delta_f_ref; F_x - F_x_ref] % - 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; 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) delta_lims = [-0.5, 0.5]; % [rad] F_x_lims = [-5e3, 5e3]; % [N] % Position Limits (Min/Max based on "map") x_lims = [ 200, 1600]; % [m] y_lims = [-200, 1000]; % [m] % 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] ]; % Simulation Parameters T_s = 0.01; % Step Size [s] T_p = 0.5; % Prediction Horizon [s] sim_stop_idx = 2.0e3; % Index in reference trajectory to stop sim % Decision Variables nstates = 6; % Number of states per prediction ninputs = 2; % Number of inputs per prediction npred; % Number of predictions npredstates; % State prediction horizon npredinputs; % Input prediction horizon ndecstates; % Total number of decision states ndecinputs; % Total number of decision inputs ndec; % Total number of decision variables % Track, Obstacle, & Reference Trajectory Information TestTrack; % Information on track boundaries and centerline Xobs_seen; % Information on seen obstacles Y_curr; % Current state Y_ref; % States of reference trajectory (Size = [steps, nstates]) U_ref; % Inputs of reference trajectory (Size = [steps, ninputs]) ref_idx; % Index of reference trajectory closest to `Y_curr` FLAG_terminate; % Binary flag indicating simulation termination % MPC & Optimization Parameters Q = [ ... % State Error Costs 1e-2; ... % x_err [m] 1e-2; ... % u_err [m/s] 1e-2; ... % y_err [m] 1e-2; ... % v_err [m/s] 1e-2; ... % psi_err [rad] 1e-2; ... % r_err [rad/s] ]; R = [ ... % Input Error Costs 1e0; ... % delta_f_err [rad] 1e-2; ... % F_x_err [N] ]; NL = [ ... % Nonlinear Constraint Cost 1e6; ... % within track bounds 1e6; ... % outside obstacles ]; options = ... optimoptions( ... 'fmincon', ... 'MaxFunctionEvaluations', 5000, ... 'SpecifyConstraintGradient', false, ... 'SpecifyObjectiveGradient', false ... ); % Constraint Parameters track_dist_fact = 1.00; % Buffer factor on distance from track boundary obs_rad_fact = 0.90; % Buffer factor on obstacle radius end %% Public Functions methods (Access = public) function obj = MPC_Class(TestTrack, Xobs_seen, Y_curr, Y_ref, U_ref) %MPC_CLASS Construct an instance of this class and % store provided track, obstacle, & trajectory information obj.TestTrack = TestTrack; obj.Xobs_seen = Xobs_seen; obj.Y_curr = Y_curr; obj.Y_ref = Y_ref; obj.U_ref = U_ref; obj.ref_idx = obj.get_ref_index(); % Calculate decision variable related quantities obj.npred = obj.T_p / obj.T_s; obj.npredstates = obj.npred + 1; obj.npredinputs = obj.npred + 1; obj.ndecstates = obj.npredstates * obj.nstates; obj.ndecinputs = obj.npredinputs * obj.ninputs; obj.ndec = obj.ndecstates + obj.ndecinputs; % Stop simulation when `sim_stop_idx` is reached obj.FLAG_terminate = 0; if obj.ref_idx > obj.sim_stop_idx disp('Reached simulation stop index') obj.FLAG_terminate = 1; end end function [Utemp, FLAG_terminate] = compute_inputs(obj) %compute_inputs Solves optimization problem to follow reference trajectory % while avoiding obstacles and staying on the track % Initialize guess to follow reference trajectory perfectly (error = 0) Z_err_init = zeros(obj.ndec, 1); % Get constraint matrices [A, B] = obj.linearized_bike_model(); [Aeq, beq] = obj.eq_cons(A, B); [Lb, Ub] = obj.bound_cons(); [ ... % `fmincon` outputs Z_err, ... % x: Solution ~, ... % fval: Objective function value at solution exitflag, ... % exitflag: Reason `fmincon` stopped ~, ... % output: Information about the optimization process ] = ... fmincon( ... % `fmincon` inputs @obj.cost_fun, ... % fun: Function to minimize Z_err_init, ... % x0: Initial point [], ... % A: Linear inequality constraints [], ... % b: Linear inequality constraints Aeq, ... % Aeq: Linear equality constraints beq, ... % beq: Linear equality constraints Lb, ... % lb: Lower bounds Ub, ... % ub: Upper bounds @obj.nonlcon, ... % nonlcon: Nonlinear constraints obj.options ... % options: Optimization options ); % Stop sim if `fmincon` failed to find feasible point if exitflag == -2 disp('No feasible point was found') obj.FLAG_terminate = 1; end % Stop sim if nonlinear constraint was violated [c, ~] = obj.nonlcon(Z_err); if any(c > 0) disp('Found point violates nonlinear inequality constraint') obj.FLAG_terminate = 1; end % NOTE: Error = Actual - Reference % => Actual = Error + Reference Z_ref = obj.get_ref_decision_variable(); Z = Z_err + Z_ref; % Construct N-by-2 vector of control inputs with timestep 0.01 seconds % to foward integrate vehicle dynamics for the next 0.5 seconds Utemp = NaN(obj.npredinputs, obj.ninputs); for i = 0:obj.npredinputs-1 idx = obj.get_input_start_idx(0); Utemp(i+1,:) = Z(idx+1:idx+obj.ninputs); end FLAG_terminate = obj.FLAG_terminate; end end %% Private Cost Function methods (Access = private) function J = cost_fun(obj, Z_err) %cost_fun Compute cost of current Z_err based % on specified state cost (Q) and input cost (R). % Create vector to hold cost values H = NaN(obj.ndec, 1); for i = 0:obj.npredstates-1 start_idx = obj.get_state_start_idx(i); H(start_idx+1:start_idx+obj.nstates) = obj.Q; end for i = 0:obj.npredinputs-1 start_idx = obj.get_input_start_idx(i); H(start_idx+1:start_idx+obj.ninputs) = obj.R; end % Create diagonal matrix from cost value vector H = diag(H); % Evaluate cost function (quadratic function) J = 0.5 * Z_err.' * H * Z_err; end end %% Private Constraint Functions methods (Access = private) function [Lb, Ub] = bound_cons(obj) %bound_cons Construct lower and upper bounds on states and inputs % using stored limits and reference trajectory at the index % closest to `Y_curr` Lb = -Inf(1, obj.ndec); % Lower Bound Ub = Inf(1, obj.ndec); % Upper Bound % NOTE: Error = Actual - Reference % Limits are defined for "Actual" states and inputs, % but our decision variable is the "Error". We have to % correct for this by subtracting reference states and % inputs from the "Actual" limits. % State Limits for i = 0:obj.npredstates-1 start_idx = obj.get_state_start_idx(i); % x position limits Lb(start_idx+1) = obj.x_lims(1) - obj.Y_ref(obj.ref_idx+i, 1); Ub(start_idx+1) = obj.x_lims(2) - obj.Y_ref(obj.ref_idx+i, 1); % y position limits Lb(start_idx+3) = obj.y_lims(1) - obj.Y_ref(obj.ref_idx+i, 3); Ub(start_idx+3) = obj.y_lims(2) - obj.Y_ref(obj.ref_idx+i, 3); end % Input Limits for i = 0:obj.npredinputs-1 start_idx = obj.get_input_start_idx(i); % delta_f input limits Lb(start_idx+1) = obj.delta_lims(1) - obj.U_ref(obj.ref_idx+i, 1); Ub(start_idx+1) = obj.delta_lims(2) - obj.U_ref(obj.ref_idx+i, 1); % F_x input limits Lb(start_idx+2) = obj.F_x_lims(1) - obj.U_ref(obj.ref_idx+i, 2); Ub(start_idx+2) = obj.F_x_lims(2) - obj.U_ref(obj.ref_idx+i, 2); end end function [Aeq, beq] = eq_cons(obj, A, B) %eq_cons Construct equality constraint matrices for % Euler discretization of linearized system % using reference trajectory at the index closest % to `Y_curr` % Build matrix for A_i*Y_i + B_i*U_i - Y_{i+1} = 0 % in the form Aeq*Z = beq % Initial Condition % NOTE: Error = Actual - Reference Y_err_init = obj.Y_curr - obj.Y_ref(obj.ref_idx, :); Aeq = zeros(obj.ndecstates, obj.ndec); beq = zeros(obj.ndecstates, 1); Aeq(1:obj.nstates, 1:obj.nstates) = eye(obj.nstates); beq(1:obj.nstates) = Y_err_init; state_idxs = obj.nstates+1:obj.nstates:obj.ndecstates; input_idxs = obj.ndecstates+1:obj.ninputs:obj.ndec; for i = 1:obj.npred % Negative identity for i+1 Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ... state_idxs(i):state_idxs(i)+obj.nstates-1) ... = -eye(obj.nstates); % A matrix for i Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ... state_idxs(i)-obj.nstates:state_idxs(i)-1) ... = A(obj.ref_idx+i-1); % B matrix for i Aeq(state_idxs(i):state_idxs(i)+obj.nstates-1, ... input_idxs(i):input_idxs(i)+obj.ninputs-1) ... = B(obj.ref_idx+i-1); end end function [c, ceq] = nonlcon(obj, Z_err) %nonlcon Construct nonlinear constraints for % vehicle to stay within track bounds and % outside of obstacles using reference % trajectory at the index closest to % `Y_curr` % No equality constraints, so leave `ceq` empty ceq = []; % Calculate number of constraints ntrack_cons = 2; % left and right track boundary nobstacle_cons = length(obj.Xobs_seen); % outside of each obstacle ncons_per_state = ntrack_cons + nobstacle_cons; ntotal_cons = obj.npredstates * ncons_per_state; % Nonlinear inequality constraints from track boundary % and obstacles applied to states c = NaN(1, ntotal_cons); cons_idx = 1; % NOTE: Error = Actual - Reference % => Actual = Error + Reference Z_ref = obj.get_ref_decision_variable(); Z = Z_err + Z_ref; % Construct constraint for each state for i = 0:obj.npredstates-1 % Get index of current state in decision variable idx = obj.get_state_start_idx(i); Y = Z(idx+1:idx+obj.nstates); % Get xy position from state vector p = [Y(1); Y(3)]; % Get indexes of closest two track border points [~,left_idxs] = mink(vecnorm(p - obj.TestTrack.bl), 2); [~,right_idxs] = mink(vecnorm(p - obj.TestTrack.br), 2); % Compute distance and direction from left and right boundaries [left_dist, left_direction] = ... obj.point_to_line( ... p, ... obj.TestTrack.bl(:,min(left_idxs)), ... obj.TestTrack.bl(:,max(left_idxs)) ... ); [right_dist, right_direction] = ... obj.point_to_line( ... p, ... obj.TestTrack.br(:,min(right_idxs)), ... obj.TestTrack.br(:,max(right_idxs)) ... ); % Construct Track Boundary Constraints % Direction from left boundary should be negative, % so <= 0 constraint is satisfied when left_direction % is negative c(cons_idx) = obj.track_dist_fact * left_dist * left_direction; cons_idx = cons_idx + 1; % Direction from right boundary should be positive, % so <= 0 constraint is satisfied when right_direction % is positive c(cons_idx) = obj.track_dist_fact * right_dist * -right_direction; cons_idx = cons_idx + 1; % Construct Obstacle Constraints for j = 1:length(obj.Xobs_seen) cen_obstacle = [ ... % Obstacle centroid mean(obj.Xobs_seen{j}(:,1)), ... mean(obj.Xobs_seen{j}(:,2)) ... ]; rad_obstacle = ... % Obstacle radius max(vecnorm(cen_obstacle - obj.Xobs_seen{j})) ... * obj.obs_rad_fact; % Model obstacle as a circle c(cons_idx) = ... (rad_obstacle)^2 ... - (p(1) - cen_obstacle(1))^2 ... - (p(2) - cen_obstacle(2))^2; cons_idx = cons_idx + 1; end end end end %% Private Kinematic Models methods (Access = private) function [A, B] = linearized_bike_model(obj) %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 `Y_curr` % Continuous-time system A_c = @(i) obj.A_c_func(i); B_c = @(i) obj.B_c_func(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 = obj.clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2)); F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2)); end function A_c = A_c_func(obj, i) %A_c Computes the continuous time `A` matrix % This function was generated by the Symbolic Math Toolbox version 9.0. % 12-Dec-2021 15:05:59 [~,u_var,~,v_var,psi_var,r_var,~,~,~,~] ... = obj.bike_model_helper( ... obj.Y_ref(obj.ref_idx+i,:), ... obj.U_ref(obj.ref_idx+i,:) ... ); t2 = cos(psi_var); t3 = sin(psi_var); A_c = reshape([ ... 0.0,0.0,0.0,0.0,0.0,0.0, ... t2,0.0,t3,-r_var,0.0,0.0, ... 0.0,0.0,0.0,0.0,0.0,0.0, ... -t3,r_var,t2,0.0,0.0,0.0, ... -t3.*u_var-t2.*v_var,0.0,t2.*u_var-t3.*v_var,0.0,0.0,0.0, ... 0.0,v_var,0.0,-u_var,1.0,0.0 ... ], ... [6,6] ... ); end function B_c = B_c_func(obj, i) %B_c_func Computes the continuous time `B` matrix % This function was generated by the Symbolic Math Toolbox version 9.0. % 12-Dec-2021 15:05:59 [~,~,~,~,~,~,delta_f_var,~,F_yf_var,~] ... = obj.bike_model_helper( ... obj.Y_ref(obj.ref_idx+i,:), ... obj.U_ref(obj.ref_idx+i,:) ... ); t2 = sin(delta_f_var); B_c = reshape([ ... 0.0,F_yf_var.*cos(delta_f_var).*(-7.142857142857143e-4), ... 0.0,F_yf_var.*t2.*(-7.142857142857143e-4), ... 0.0,F_yf_var.*t2.*(-5.061867266591676e-4), ... 0.0,1.0./7.0e+2, ... 0.0,0.0, ... 0.0,0.0 ... ], ... [6,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 % the full decision variable. Assumes `i` starts % at 0. idx = obj.nstates*i; end function idx = get_input_start_idx(obj, i) %get_input_start_idx Calculates starting index of input i in % the full decision variable. Assumes `i` starts % at 0. idx = (obj.npred+1)*obj.nstates + obj.ninputs*i; end function idx = get_ref_index(obj) %get_ref_index Finds index of position in reference trajectory % that is closest (based on Euclidean distance) to position % in `Y_curr` % Get position (x,y) from current state pos = [obj.Y_curr(1), obj.Y_curr(3)]; % Get positions (x,y) from reference trajectory pos_ref = obj.Y_ref(:,[1,3]); % Calculate Euclidean distance between current position and % reference trajectory positions dist = vecnorm(pos - pos_ref, 2, 2); % Get index of reference position closest to current position [~, idx] = min(dist); end function Z_ref = get_ref_decision_variable(obj) %get_ref_decision_variable Constructs decision variable % from reference trajectory states and inputs starting % at index closest to `Y_curr` Z_ref = zeros(obj.ndec, 1); for i = 0:obj.npredstates-1 start_idx = obj.get_state_start_idx(i); Z_ref(start_idx+1:start_idx+obj.nstates) ... = obj.Y_ref(obj.ref_idx+i, :); end for i = 0:obj.npredinputs-1 start_idx = obj.get_input_start_idx(i); Z_ref(start_idx+1:start_idx+obj.ninputs) ... = obj.U_ref(obj.ref_idx+i, :); end end end %% Static Helper Functions methods (Static) function clamped_val = clamp(val, min, max) %clamp Limits a value to range: min <= val <= max clamped_val = val; if clamped_val < min clamped_val = min; elseif clamped_val > max clamped_val = max; end end function [dist, direction] = point_to_line(pt, v1, v2) %point_to_line Computes shortest distance from % a point pt to a line defined by v1 & v2 % and direction (+/- 1) based on axis orthogonal to xy % plane being pointed upwards such that CCW rotation % is positive % Convert 2D column vectors to 3D column vectors pt = [pt; 0]; v1 = [v1; 0]; v2 = [v2; 0]; % Compute distance a = v2 - v1; % vector from v1 to v2 b = pt - v1; % vector from v1 to pt dist = norm(cross(a,b)) / norm(a); % Compute direction as sign of cross product cross_p = cross(a,b); direction = sign(cross_p(3)); end end end