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

Continuous Track/Boundary Constraints

Browse Source 
- Adjust input error cost for steering angle
- Increase `fmincon` maximum function evaluations
- Add buffer factors for track boundary distance
  and obstacle radius
- Add intermediate variables for calculating the
  number of constraints
- Change discrete `inpolygon` constraints to continuous
  function based constraints where obstacles are modeled
  as circles and distance/direction from track boundary
  is computed
- Add `point_to_line` function for computing track boundary
  quantities
This commit is contained in:
Sravan Balaji
2021-12-13 11:59:43 -05:00
parent 7d502fa8f5
commit 3d3c53fcd1
1 changed files with 76 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
Deliverables/MPC_Class.m
View File

@@ -89,7 +89,7 @@ classdef MPC_Class
1e-2; ... % r_err [rad/s] 1e-2; ... % r_err [rad/s]
]; ];
R = [ ... % Input Error Costs R = [ ... % Input Error Costs
1e-2; ... % delta_f_err [rad] 1e0; ... % delta_f_err [rad]
1e-2; ... % F_x_err [N] 1e-2; ... % F_x_err [N]
]; ];
NL = [ ... % Nonlinear Constraint Cost NL = [ ... % Nonlinear Constraint Cost
@@ -99,10 +99,14 @@ classdef MPC_Class
options = ... options = ...
optimoptions( ... optimoptions( ...
'fmincon', ... 'fmincon', ...
'MaxFunctionEvaluations', 4000, ... 'MaxFunctionEvaluations', 5000, ...
'SpecifyConstraintGradient', false, ... 'SpecifyConstraintGradient', false, ...
'SpecifyObjectiveGradient', 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 end
%% Public Functions %% Public Functions
@@ -315,9 +319,15 @@ classdef MPC_Class
% No equality constraints, so leave `ceq` empty % No equality constraints, so leave `ceq` empty
ceq = []; 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 % Nonlinear inequality constraints from track boundary
% and obstacles applied to states % and obstacles applied to states
c = NaN(1, obj.npredstates*(1+length(obj.Xobs_seen))); c = NaN(1, ntotal_cons);
cons_idx = 1; cons_idx = 1;
% NOTE: Error = Actual - Reference % NOTE: Error = Actual - Reference
@@ -334,42 +344,53 @@ classdef MPC_Class
% Get xy position from state vector % Get xy position from state vector
p = [Y(1); Y(3)]; p = [Y(1); Y(3)];
% Construct track polygon from all boundary points % Get indexes of closest two track border points
bl_idx_start = 1; [~,left_idxs] = mink(vecnorm(p - obj.TestTrack.bl), 2);
bl_idx_end = size(obj.TestTrack.bl,2); [~,right_idxs] = mink(vecnorm(p - obj.TestTrack.br), 2);
br_idx_start = 1;
br_idx_end = size(obj.TestTrack.br,2);
boundary_pts = [ ... % Compute distance and direction from left and right boundaries
obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ... [left_dist, left_direction] = ...
obj.TestTrack.br(:,br_idx_end:-1:br_idx_start) ... obj.point_to_line( ...
]; p, ...
xv_track = boundary_pts(1,:); obj.TestTrack.bl(:,min(left_idxs)), ...
yv_track = boundary_pts(2,:); 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)) ...
);
% Check if p is within track polygon % Construct Track Boundary Constraints
in_track = inpolygon(p(1), p(2), xv_track, yv_track);
if ~in_track % Direction from left boundary should be negative,
% Position not inside track % so <= 0 constraint is satisfied when left_direction
c(cons_idx) = obj.NL(1); % c(Z_err) > 0, nonlinear inequality constraint violated % is negative
else c(cons_idx) = obj.track_dist_fact * left_dist * left_direction;
c(cons_idx) = -obj.NL(1); % c(Z_err) <= 0, nonlinear inequality constraint satisfied
end
cons_idx = cons_idx + 1; cons_idx = cons_idx + 1;
for j = 1:length(obj.Xobs_seen) % Direction from right boundary should be positive,
% Check if position is in or on each obstacle % so <= 0 constraint is satisfied when right_direction
xv_obstacle = obj.Xobs_seen{j}(:,1); % is positive
yv_obstacle = obj.Xobs_seen{j}(:,2); c(cons_idx) = obj.track_dist_fact * right_dist * -right_direction;
[in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle); cons_idx = cons_idx + 1;
if in_obstacle || on_obstacle % Construct Obstacle Constraints
% Point in or on obstacle for j = 1:length(obj.Xobs_seen)
c(cons_idx) = obj.NL(2); % c(Z_err) > 0, nonlinear inequality constraint violated cen_obstacle = [ ... % Obstacle centroid
else mean(obj.Xobs_seen{j}(:,1)), ...
c(cons_idx) = -obj.NL(2); % c(Z_err) <= 0, nonlinear inequality constraint satisfied mean(obj.Xobs_seen{j}(:,2)) ...
end ];
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; cons_idx = cons_idx + 1;
end end
end end
@@ -562,6 +583,28 @@ classdef MPC_Class
clamped_val = max; clamped_val = max;
end end
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
end end