sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-19 17:22:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Playing with Nonlinear Constraint Costs

Browse Source 
- Drastically increase nonlinear constraint violation cost (I think)
- Make state & input costs much smaller than constraint costs
- Add condition to stop simulation if nonlinear constraint
  is not satisfied (doesn't seem to work for going off track)
- Create separate nonlinear constraints for each state to check
  if it is on track and if it doesn't intersect each obstacle
- Change indexing of nonlinear constraints
This commit is contained in:
Sravan Balaji
2021-12-13 07:16:27 -05:00
parent 69c704e66b
commit 91fc8ff4c3
1 changed files with 37 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
Deliverables/MPC_Class.m
View File

@@ -56,7 +56,7 @@ classdef MPC_Class
% Simulation Parameters % Simulation Parameters
T_s = 0.01; % Step Size [s] T_s = 0.01; % Step Size [s]
T_p = 0.5; % Prediction Horizon [s] T_p = 0.5; % Prediction Horizon [s]
sim_stop_idx = 1.5e3; % Index in reference trajectory to stop sim sim_stop_idx = 2.0e3; % Index in reference trajectory to stop sim
% Decision Variables % Decision Variables
nstates = 6; % Number of states per prediction nstates = 6; % Number of states per prediction
@@ -81,17 +81,22 @@ classdef MPC_Class
% MPC & Optimization Parameters % MPC & Optimization Parameters
Q = [ ... % State Error Costs Q = [ ... % State Error Costs
1; ... % x_err [m] 1e-2; ... % x_err [m]
1; ... % u_err [m/s] 1e-2; ... % u_err [m/s]
1; ... % y_err [m] 1e-2; ... % y_err [m]
1; ... % v_err [m/s] 1e-2; ... % v_err [m/s]
1; ... % psi_err [rad] 1e-2; ... % psi_err [rad]
1; ... % r_err [rad/s] 1e-2; ... % r_err [rad/s]
]; ];
R = [ ... % Input Error Costs R = [ ... % Input Error Costs
0.1; ... % delta_f_err [rad] 1e-2; ... % delta_f_err [rad]
0.01; ... % F_x_err [N] 1e-2; ... % F_x_err [N]
]; ];
NL = [ ... % Nonlinear Constraint Cost
1e6; ... % within track bounds
1e6; ... % outside obstacles
];
options = ... options = ...
optimoptions( ... optimoptions( ...
'fmincon', ... 'fmincon', ...
@@ -149,9 +154,9 @@ classdef MPC_Class
[ ... % `fmincon` outputs [ ... % `fmincon` outputs
Z_err, ... % x: Solution Z_err, ... % x: Solution
J_val, ... % fval: Objective function value at solution ~, ... % fval: Objective function value at solution
exitflag, ... % exitflag: Reason `fmincon` stopped exitflag, ... % exitflag: Reason `fmincon` stopped
output, ... % output: Information about the optimization process ~, ... % output: Information about the optimization process
] = ... ] = ...
fmincon( ... % `fmincon` inputs fmincon( ... % `fmincon` inputs
@obj.cost_fun, ... % fun: Function to minimize @obj.cost_fun, ... % fun: Function to minimize
@@ -166,11 +171,19 @@ classdef MPC_Class
obj.options ... % options: Optimization options obj.options ... % options: Optimization options
); );
% Check `fmincon` exitflag
if exitflag == -2 if exitflag == -2
disp('No feasible point was found') disp('No feasible point was found')
obj.FLAG_terminate = 1; obj.FLAG_terminate = 1;
end end
% Check if nonlinear constraint was satisfied
[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 % NOTE: Error = Actual - Reference
% => Actual = Error + Reference % => Actual = Error + Reference
Z_ref = obj.get_ref_decision_variable(); Z_ref = obj.get_ref_decision_variable();
@@ -310,7 +323,8 @@ classdef MPC_Class
% Nonlinear inequality constraints from track boundary % Nonlinear inequality constraints from track boundary
% and obstacles applied to states % and obstacles applied to states
c = NaN(1, obj.npredstates); c = NaN(1, obj.npredstates*(1+length(obj.Xobs_seen)));
cons_idx = 1;
% NOTE: Error = Actual - Reference % NOTE: Error = Actual - Reference
% => Actual = Error + Reference % => Actual = Error + Reference
@@ -348,13 +362,13 @@ classdef MPC_Class
if ~in_track if ~in_track
% Position not inside track % Position not inside track
c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated c(cons_idx) = obj.NL(1); % c(Z_err) > 0, nonlinear inequality constraint violated
else
% Skip to next constraint c(cons_idx) = -obj.NL(1); % c(Z_err) <= 0, nonlinear inequality constraint satisfied
continue;
end end
cons_idx = cons_idx + 1;
for j = 1:size(obj.Xobs_seen,2) for j = 1:length(obj.Xobs_seen)
% Check if position is in or on each obstacle % Check if position is in or on each obstacle
xv_obstacle = obj.Xobs_seen{j}(:,1); xv_obstacle = obj.Xobs_seen{j}(:,1);
yv_obstacle = obj.Xobs_seen{j}(:,2); yv_obstacle = obj.Xobs_seen{j}(:,2);
@@ -362,16 +376,11 @@ classdef MPC_Class
if in_obstacle || on_obstacle if in_obstacle || on_obstacle
% Point in or on obstacle % Point in or on obstacle
c(i+1) = 1; % c(Z_err) > 0, nonlinear inequality constraint violated c(cons_idx) = obj.NL(2); % c(Z_err) > 0, nonlinear inequality constraint violated
else
% Skip remaining obstacle checking c(cons_idx) = -obj.NL(2); % c(Z_err) <= 0, nonlinear inequality constraint satisfied
break;
end end
end cons_idx = cons_idx + 1;
if isnan(c(i+1))
% If value not set, no constraints violated
c(i+1) = -1; % c(Z_err) <= 0, nonlinear inequality constraint satisfied
end end
end end
end end