sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-19 09:12: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

65
Deliverables/MPC_Class.m
View File

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