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]
];
R = [ ... % Input Error Costs
1e-2; ... % delta_f_err [rad]
1e0; ... % delta_f_err [rad]
1e-2; ... % F_x_err [N]
];
NL = [ ... % Nonlinear Constraint Cost
@@ -99,10 +99,14 @@ classdef MPC_Class
options = ...
optimoptions( ...
'fmincon', ...
'MaxFunctionEvaluations', 4000, ...
'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
@@ -315,9 +319,15 @@ classdef MPC_Class
% 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, obj.npredstates*(1+length(obj.Xobs_seen)));
c = NaN(1, ntotal_cons);
cons_idx = 1;
% NOTE: Error = Actual - Reference
@@ -334,42 +344,53 @@ classdef MPC_Class
% Get xy position from state vector
p = [Y(1); Y(3)];
% Construct track polygon from all boundary points
bl_idx_start = 1;
bl_idx_end = size(obj.TestTrack.bl,2);
br_idx_start = 1;
br_idx_end = size(obj.TestTrack.br,2);
% 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);
boundary_pts = [ ...
obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...
obj.TestTrack.br(:,br_idx_end:-1:br_idx_start) ...
];
xv_track = boundary_pts(1,:);
yv_track = boundary_pts(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)) ...
);
% Check if p is within track polygon
in_track = inpolygon(p(1), p(2), xv_track, yv_track);
% Construct Track Boundary Constraints
if ~in_track
% Position not inside track
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
% 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;
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);
[in_obstacle, on_obstacle] = inpolygon(p(1), p(2), xv_obstacle, yv_obstacle);
% 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;
if in_obstacle || on_obstacle
% Point in or on obstacle
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
% 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
@@ -562,6 +583,28 @@ classdef MPC_Class
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