mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-17 16:42:44 +00:00
Add Gradients for Cost Function & Nonlinear Constraints
- Set gradient options to true for `fmincon` optimizer - Play with optimizer limits a bit - Add gradient computation for cost function - Add gradient computation for track & obstacle constraints - Compute gradients offline in `miscellaneous_stuff.m` - Update `point_to_line` to output signed distance
This commit is contained in:
Sravan Balaji
@@ -99,14 +99,15 @@ classdef MPC_Class
|
|||||||
options = ...
|
options = ...
|
||||||
optimoptions( ...
|
optimoptions( ...
|
||||||
'fmincon', ...
|
'fmincon', ...
|
||||||
'MaxFunctionEvaluations', 5000, ...
|
'MaxFunctionEvaluations', 3000, ...
|
||||||
'SpecifyConstraintGradient', false, ...
|
'MaxIterations', 3000, ...
|
||||||
'SpecifyObjectiveGradient', false ...
|
'SpecifyConstraintGradient', true, ...
|
||||||
|
'SpecifyObjectiveGradient', true ...
|
||||||
);
|
);
|
||||||
|
|
||||||
% Constraint Parameters
|
% Constraint Parameters
|
||||||
track_dist_fact = 1.00; % Buffer factor on distance from track boundary
|
track_dist_fact = 1.00; % Buffer factor on distance from track boundary
|
||||||
obs_rad_fact = 0.90; % Buffer factor on obstacle radius
|
obs_rad_fact = 1.00; % Buffer factor on obstacle radius
|
||||||
end
|
end
|
||||||
|
|
||||||
%% Public Functions
|
%% Public Functions
|
||||||
@@ -201,7 +202,7 @@ classdef MPC_Class
|
|||||||
|
|
||||||
%% Private Cost Function
|
%% Private Cost Function
|
||||||
methods (Access = private)
|
methods (Access = private)
|
||||||
function J = cost_fun(obj, Z_err)
|
function [J, dJ] = cost_fun(obj, Z_err)
|
||||||
%cost_fun Compute cost of current Z_err based
|
%cost_fun Compute cost of current Z_err based
|
||||||
% on specified state cost (Q) and input cost (R).
|
% on specified state cost (Q) and input cost (R).
|
||||||
|
|
||||||
@@ -223,6 +224,7 @@ classdef MPC_Class
|
|||||||
|
|
||||||
% Evaluate cost function (quadratic function)
|
% Evaluate cost function (quadratic function)
|
||||||
J = 0.5 * Z_err.' * H * Z_err;
|
J = 0.5 * Z_err.' * H * Z_err;
|
||||||
|
dJ = H * Z_err;
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
@@ -309,7 +311,7 @@ classdef MPC_Class
|
|||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
function [c, ceq] = nonlcon(obj, Z_err)
|
function [c, ceq, gradc, gradceq] = nonlcon(obj, Z_err)
|
||||||
%nonlcon Construct nonlinear constraints for
|
%nonlcon Construct nonlinear constraints for
|
||||||
% vehicle to stay within track bounds and
|
% vehicle to stay within track bounds and
|
||||||
% outside of obstacles using reference
|
% outside of obstacles using reference
|
||||||
@@ -318,6 +320,7 @@ classdef MPC_Class
|
|||||||
|
|
||||||
% No equality constraints, so leave `ceq` empty
|
% No equality constraints, so leave `ceq` empty
|
||||||
ceq = [];
|
ceq = [];
|
||||||
|
gradceq = [];
|
||||||
|
|
||||||
% Calculate number of constraints
|
% Calculate number of constraints
|
||||||
ntrack_cons = 2; % left and right track boundary
|
ntrack_cons = 2; % left and right track boundary
|
||||||
@@ -328,6 +331,7 @@ 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, ntotal_cons);
|
c = NaN(1, ntotal_cons);
|
||||||
|
gradc = zeros(obj.ndec, ntotal_cons);
|
||||||
cons_idx = 1;
|
cons_idx = 1;
|
||||||
|
|
||||||
% NOTE: Error = Actual - Reference
|
% NOTE: Error = Actual - Reference
|
||||||
@@ -339,41 +343,59 @@ classdef MPC_Class
|
|||||||
for i = 0:obj.npredstates-1
|
for i = 0:obj.npredstates-1
|
||||||
% Get index of current state in decision variable
|
% Get index of current state in decision variable
|
||||||
idx = obj.get_state_start_idx(i);
|
idx = obj.get_state_start_idx(i);
|
||||||
Y = Z(idx+1:idx+obj.nstates);
|
|
||||||
|
|
||||||
% Get xy position from state vector
|
% Get xy position from state vector
|
||||||
p = [Y(1); Y(3)];
|
p = [Z(idx+1); Z(idx+3)];
|
||||||
|
|
||||||
|
% Get reference and error state information
|
||||||
|
x_ref = Z_ref(idx+1);
|
||||||
|
y_ref = Z_ref(idx+3);
|
||||||
|
x_err = Z_err(idx+1);
|
||||||
|
y_err = Z_err(idx+3);
|
||||||
|
|
||||||
% Get indexes of closest two track border points
|
% Get indexes of closest two track border points
|
||||||
[~,left_idxs] = mink(vecnorm(p - obj.TestTrack.bl), 2);
|
[~,left_idxs] = mink(vecnorm(p - obj.TestTrack.bl), 2);
|
||||||
[~,right_idxs] = mink(vecnorm(p - obj.TestTrack.br), 2);
|
[~,right_idxs] = mink(vecnorm(p - obj.TestTrack.br), 2);
|
||||||
|
|
||||||
% Compute distance and direction from left and right boundaries
|
v1_left = obj.TestTrack.bl(:,min(left_idxs));
|
||||||
[left_dist, left_direction] = ...
|
v2_left = obj.TestTrack.bl(:,max(left_idxs));
|
||||||
obj.point_to_line( ...
|
v1_right = obj.TestTrack.br(:,min(right_idxs));
|
||||||
p, ...
|
v2_right = obj.TestTrack.br(:,max(right_idxs));
|
||||||
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)) ...
|
|
||||||
);
|
|
||||||
|
|
||||||
% Construct Track Boundary Constraints
|
% Construct Track Boundary Constraints
|
||||||
|
|
||||||
% Direction from left boundary should be negative,
|
% Direction from left boundary should be negative,
|
||||||
% so <= 0 constraint is satisfied when left_direction
|
% so <= 0 constraint is satisfied when left_signed_dist
|
||||||
% is negative
|
% is negative
|
||||||
c(cons_idx) = obj.track_dist_fact * left_dist * left_direction;
|
left_signed_dist = obj.point_to_line(p, v1_left, v2_left);
|
||||||
|
c(cons_idx) = obj.track_dist_fact * left_signed_dist;
|
||||||
|
gradc(idx+1, cons_idx) = ...
|
||||||
|
obj.point_to_line_grad_x( ...
|
||||||
|
v1_left(1), v1_left(2), v2_left(1), v2_left(2), ...
|
||||||
|
x_err, x_ref, y_err, y_ref ...
|
||||||
|
);
|
||||||
|
gradc(idx+3, cons_idx) = ...
|
||||||
|
obj.point_to_line_grad_y( ...
|
||||||
|
v1_left(1), v1_left(2), v2_left(1), v2_left(2), ...
|
||||||
|
x_err, x_ref, y_err, y_ref ...
|
||||||
|
);
|
||||||
cons_idx = cons_idx + 1;
|
cons_idx = cons_idx + 1;
|
||||||
|
|
||||||
% Direction from right boundary should be positive,
|
% Direction from right boundary should be positive,
|
||||||
% so <= 0 constraint is satisfied when right_direction
|
% so <= 0 constraint is satisfied when right_signed_dist
|
||||||
% is positive
|
% is positive
|
||||||
c(cons_idx) = obj.track_dist_fact * right_dist * -right_direction;
|
right_signed_dist = obj.point_to_line(p, v1_right, v2_right);
|
||||||
|
c(cons_idx) = obj.track_dist_fact * -right_signed_dist;
|
||||||
|
gradc(idx+1, cons_idx) = ...
|
||||||
|
obj.point_to_line_grad_x( ...
|
||||||
|
v1_right(1), v1_right(2), v2_right(1), v2_right(2), ...
|
||||||
|
x_err, x_ref, y_err, y_ref ...
|
||||||
|
);
|
||||||
|
gradc(idx+3, cons_idx) = ...
|
||||||
|
obj.point_to_line_grad_y( ...
|
||||||
|
v1_right(1), v1_right(2), v2_right(1), v2_right(2), ...
|
||||||
|
x_err, x_ref, y_err, y_ref ...
|
||||||
|
);
|
||||||
cons_idx = cons_idx + 1;
|
cons_idx = cons_idx + 1;
|
||||||
|
|
||||||
% Construct Obstacle Constraints
|
% Construct Obstacle Constraints
|
||||||
@@ -387,10 +409,9 @@ classdef MPC_Class
|
|||||||
* obj.obs_rad_fact;
|
* obj.obs_rad_fact;
|
||||||
|
|
||||||
% Model obstacle as a circle
|
% Model obstacle as a circle
|
||||||
c(cons_idx) = ...
|
c(cons_idx) = obj.obstacle_cons(p, rad_obstacle, cen_obstacle);
|
||||||
(rad_obstacle)^2 ...
|
gradc(idx+1, cons_idx) = obj.obstacle_grad_x(cen_obstacle(1), x_err, x_ref);
|
||||||
- (p(1) - cen_obstacle(1))^2 ...
|
gradc(idx+3, cons_idx) = obj.obstacle_grad_y(cen_obstacle(2), y_err, y_ref);
|
||||||
- (p(2) - cen_obstacle(2))^2;
|
|
||||||
cons_idx = cons_idx + 1;
|
cons_idx = cons_idx + 1;
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
@@ -584,27 +605,114 @@ classdef MPC_Class
|
|||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
function [dist, direction] = point_to_line(pt, v1, v2)
|
function signed_dist = point_to_line(pt, v1, v2)
|
||||||
%point_to_line Computes shortest distance from
|
%point_to_line Computes shortest distance from
|
||||||
% a point pt to a line defined by v1 & v2
|
% a point pt to a line defined by v1 & v2
|
||||||
% and direction (+/- 1) based on axis orthogonal to xy
|
% with direction indicated by the sign where
|
||||||
% plane being pointed upwards such that CCW rotation
|
% CCW rotation is positive
|
||||||
% is positive
|
|
||||||
|
|
||||||
% Convert 2D column vectors to 3D column vectors
|
% Convert 2D column vectors to 3D column vectors
|
||||||
pt = [pt; 0];
|
pt = [pt; 0];
|
||||||
v1 = [v1; 0];
|
v1 = [v1; 0];
|
||||||
v2 = [v2; 0];
|
v2 = [v2; 0];
|
||||||
|
|
||||||
% Compute distance
|
% Compute vectors
|
||||||
a = v2 - v1; % vector from v1 to v2
|
a = v2 - v1; % vector from v1 to v2
|
||||||
b = pt - v1; % vector from v1 to pt
|
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);
|
cross_p = cross(a,b);
|
||||||
|
|
||||||
|
% Compute distance & direction
|
||||||
|
dist = norm(cross_p) / norm(a);
|
||||||
direction = sign(cross_p(3));
|
direction = sign(cross_p(3));
|
||||||
|
signed_dist = dist * direction;
|
||||||
|
end
|
||||||
|
|
||||||
|
function grad = point_to_line_grad_x(v1_x, v1_y, v2_x, v2_y, x_err, x_ref, y_err, y_ref)
|
||||||
|
%point_to_line_grad_x Computes the `x_err` gradient of
|
||||||
|
% point_to_line for track boundary constraint
|
||||||
|
|
||||||
|
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||||
|
% 13-Dec-2021 13:46:47
|
||||||
|
|
||||||
|
t2 = -v1_x;
|
||||||
|
t3 = -v1_y;
|
||||||
|
t4 = -v2_x;
|
||||||
|
t5 = -v2_y;
|
||||||
|
t6 = t4+v1_x;
|
||||||
|
t7 = t5+v1_y;
|
||||||
|
t10 = t2+x_err+x_ref;
|
||||||
|
t11 = t3+y_err+y_ref;
|
||||||
|
t8 = abs(t6);
|
||||||
|
t9 = abs(t7);
|
||||||
|
t14 = t7.*t10;
|
||||||
|
t15 = t6.*t11;
|
||||||
|
t12 = t8.^2;
|
||||||
|
t13 = t9.^2;
|
||||||
|
t16 = -t15;
|
||||||
|
t17 = t12+t13;
|
||||||
|
t19 = t14+t16;
|
||||||
|
t18 = 1.0./sqrt(t17);
|
||||||
|
t20 = abs(t19);
|
||||||
|
t21 = t20.^2;
|
||||||
|
grad = t7.*t18.*sqrt(t21).*dirac(t19).*2.0+t7.*t18.*t20.*1.0./sqrt(t21).*sign(t19).^2;
|
||||||
|
end
|
||||||
|
|
||||||
|
function grad = point_to_line_grad_y(v1_x, v1_y, v2_x, v2_y, x_err, x_ref, y_err, y_ref)
|
||||||
|
%point_to_line_grad_y Computes the `y_err` gradient of
|
||||||
|
% point_to_line for track boundary constraint
|
||||||
|
|
||||||
|
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||||
|
% 13-Dec-2021 13:46:48
|
||||||
|
|
||||||
|
t2 = -v1_x;
|
||||||
|
t3 = -v1_y;
|
||||||
|
t4 = -v2_x;
|
||||||
|
t5 = -v2_y;
|
||||||
|
t6 = t4+v1_x;
|
||||||
|
t7 = t5+v1_y;
|
||||||
|
t10 = t2+x_err+x_ref;
|
||||||
|
t11 = t3+y_err+y_ref;
|
||||||
|
t8 = abs(t6);
|
||||||
|
t9 = abs(t7);
|
||||||
|
t14 = t7.*t10;
|
||||||
|
t15 = t6.*t11;
|
||||||
|
t12 = t8.^2;
|
||||||
|
t13 = t9.^2;
|
||||||
|
t16 = -t15;
|
||||||
|
t17 = t12+t13;
|
||||||
|
t19 = t14+t16;
|
||||||
|
t18 = 1.0./sqrt(t17);
|
||||||
|
t20 = abs(t19);
|
||||||
|
t21 = t20.^2;
|
||||||
|
grad = t6.*t18.*sqrt(t21).*dirac(t19).*-2.0-t6.*t18.*t20.*1.0./sqrt(t21).*sign(t19).^2;
|
||||||
|
end
|
||||||
|
|
||||||
|
function val = obstacle_cons(p, radius, centroid)
|
||||||
|
%obstacle_cons Computes the value of obstacle constraint
|
||||||
|
% value where obstacle is modeled as a circle with
|
||||||
|
% specified radius and centroid
|
||||||
|
|
||||||
|
val = radius^2 - (p(1) - centroid(1))^2 - (p(2) - centroid(2))^2;
|
||||||
|
end
|
||||||
|
|
||||||
|
function grad = obstacle_grad_x(cen_obstacle_x,x_err,x_ref)
|
||||||
|
%obstacle_grad_x Computes the `x_err` gradient of
|
||||||
|
% obstacle_cons for obstacle constraint
|
||||||
|
|
||||||
|
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||||
|
% 13-Dec-2021 14:05:24
|
||||||
|
|
||||||
|
grad = cen_obstacle_x.*2.0-x_err.*2.0-x_ref.*2.0;
|
||||||
|
end
|
||||||
|
|
||||||
|
function grad = obstacle_grad_y(cen_obstacle_y,y_err,y_ref)
|
||||||
|
%obstacle_grad_y Computes the `y_err` gradient of
|
||||||
|
% obstacle_cons for obstacle constraint
|
||||||
|
|
||||||
|
% This function was generated by the Symbolic Math Toolbox version 9.0.
|
||||||
|
% 13-Dec-2021 14:05:25
|
||||||
|
|
||||||
|
grad = cen_obstacle_y.*2.0-y_err.*2.0-y_ref.*2.0;
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|||||||
@@ -12,7 +12,7 @@ plot(Y_test(:,1), Y_test(:,3), 'k-')
|
|||||||
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.')
|
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.')
|
||||||
plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.')
|
plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.')
|
||||||
|
|
||||||
%% Part 2 Symbolic Jacobians
|
%% Part 2 Symbolic Jacobians for Linearized System
|
||||||
close all;
|
close all;
|
||||||
clear all;
|
clear all;
|
||||||
clc;
|
clc;
|
||||||
@@ -66,3 +66,46 @@ B_c_symb = [ ...
|
|||||||
|
|
||||||
A_c = matlabFunction(A_c_symb, 'File', 'A_c_func');
|
A_c = matlabFunction(A_c_symb, 'File', 'A_c_func');
|
||||||
B_c = matlabFunction(B_c_symb, 'File', 'B_c_func');
|
B_c = matlabFunction(B_c_symb, 'File', 'B_c_func');
|
||||||
|
|
||||||
|
%% Part 2 Symbolic Jacobians for Track Constraint
|
||||||
|
close all;
|
||||||
|
clear all;
|
||||||
|
clc;
|
||||||
|
|
||||||
|
syms x_err y_err x_ref y_ref v1_x v1_y v2_x v2_y
|
||||||
|
x = x_err + x_ref;
|
||||||
|
y = y_err + y_ref;
|
||||||
|
|
||||||
|
pt = [x; y; 0];
|
||||||
|
v1 = [v1_x; v1_y; 0];
|
||||||
|
v2 = [v2_x; v2_y; 0];
|
||||||
|
|
||||||
|
% Compute vectors
|
||||||
|
a = v2 - v1; % vector from v1 to v2
|
||||||
|
b = pt - v1; % vector from v1 to pt
|
||||||
|
cross_p = cross(a,b);
|
||||||
|
|
||||||
|
% Compute distance & direction
|
||||||
|
dist = norm(cross_p) / norm(a);
|
||||||
|
direction = sign(cross_p(3));
|
||||||
|
signed_dist = dist * direction;
|
||||||
|
|
||||||
|
temp1 = matlabFunction(jacobian(signed_dist, x_err), 'File', 'signed_dist_x_err');
|
||||||
|
temp2 = matlabFunction(jacobian(signed_dist, y_err), 'File', 'signed_dist_y_err');
|
||||||
|
|
||||||
|
%% Part 2 Symbolic Jacobians for Obstacle Constraint
|
||||||
|
close all;
|
||||||
|
clear all;
|
||||||
|
clc;
|
||||||
|
|
||||||
|
syms x_err y_err x_ref y_ref rad_obstacle cen_obstacle_x cen_obstacle_y
|
||||||
|
x = x_err + x_ref;
|
||||||
|
y = y_err + y_ref;
|
||||||
|
|
||||||
|
obstacle_func = ...
|
||||||
|
(rad_obstacle)^2 ...
|
||||||
|
- (x - cen_obstacle_x)^2 ...
|
||||||
|
- (y - cen_obstacle_y)^2;
|
||||||
|
|
||||||
|
temp1 = matlabFunction(jacobian(obstacle_func, x_err), 'File', 'obstacle_x_err');
|
||||||
|
temp2 = matlabFunction(jacobian(obstacle_func, y_err), 'File', 'obstacle_y_err');
|
||||||
Reference in New Issue
Block a user