From 3d3c53fcd11a1df3cde3aeae3de6714843f373c2 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Mon, 13 Dec 2021 11:59:43 -0500 Subject: [PATCH] Continuous Track/Boundary Constraints - 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 --- Deliverables/MPC_Class.m | 109 +++++++++++++++++++++++++++------------ 1 file changed, 76 insertions(+), 33 deletions(-) diff --git a/Deliverables/MPC_Class.m b/Deliverables/MPC_Class.m index 8dc7c36..a49c61e 100644 --- a/Deliverables/MPC_Class.m +++ b/Deliverables/MPC_Class.m @@ -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