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

Position Limits & Update bound_cons

Browse Source 
- Add max/min position limits
- Delete `eq_cons` function for now
- Update `bound_cons` to enforce limit and
  max/min position limits
- Rename `nonlincon` to `road_obstacle_cons`
- Parameterize index search around closest
  road boundary point in `road_obstacle_cons`
This commit is contained in:
Sravan Balaji
2021-12-09 17:52:41 -05:00
parent 5218d7fbec
commit 729fd4fc05
1 changed files with 30 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
Experimentation/part2_MPC_controller.m
View File

@@ -29,6 +29,11 @@ global delta_lims Fx_lims
delta_lims = [-0.5, 0.5];
Fx_lims = [-5e3, 5e3];
% Position Limits
global x_lims y_lims
x_lims = [200, 1600];
y_lims = [-200, 1000];
% Initial Conditions (Equation 15)
state_init = [ ...
287; ... % x [m]
@@ -208,40 +213,35 @@ function x0vec = initvec(x0, u0)
end
%% mpc functions
function [Aeq,beq] = eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
%build matrix for A_i*x_i+B_i*u_i-x_{i+1}=0
%in the form Aeq*z=beq
%initial_idx specifies the time index of initial condition from the reference trajectory
%A and B are function handles above
function [Lb, Ub] = bound_cons(idx, X_ref, U_ref)
global num_preds num_states num_inputs x_lims y_lims delta_lims Fx_lims
%initial condition
x_initial=x_initial(:);
% initial_idx is the index along uref the initial condition is at
Lb = -Inf(num_preds*(num_states+num_inputs), 1);
Ub = Inf(num_preds*(num_states+num_inputs), 1);
%size of decision variable and size of part holding states
zsize=(npred+1)*nstates+npred*ninputs;
xsize=(npred+1)*nstates;
for i = 0:(num_preds-1)
start_idx = get_start_idx(i);
Aeq=zeros(xsize,zsize);
Aeq(1:nstates,1:nstates)=eye(nstates); %initial condition
beq=zeros(xsize,1);
beq(1:nstates)=x_initial;
% x
Lb(start_idx+1) = x_lims(1) - X_ref(idx+i, 1);
Ub(start_idx+1) = x_lims(2) - X_ref(idx+i, 1);
state_idxs=nstates+1:nstates:xsize;
input_idxs=xsize+1:ninputs:zsize;
% y
Lb(start_idx+3) = y_lims(1) - X_ref(idx+i, 3);
Ub(start_idx+3) = y_lims(2) - X_ref(idx+i, 3);
for i=1:npred
%negative identity for i+1
Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i):state_idxs(i)+nstates-1)=-eye(nstates);
% delta
Lb(start_idx+num_states+1) = delta_lims(1) - U_ref(idx+i, 1);
Ub(start_idx+num_states+1) = delta_lims(2) - U_ref(idx+i, 1);
%A matrix for i
Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i)-nstates:state_idxs(i)-1)=A(initial_idx+i-1);
%B matrix for i
Aeq(state_idxs(i):state_idxs(i)+nstates-1,input_idxs(i):input_idxs(i)+ninputs-1)=B(initial_idx+i-1);
% F_x
Lb(start_idx+num_states+2) = Fx_lims(1) - U_ref(idx+1, 2);
Ub(start_idx+num_states+2) = Fx_lims(2) - U_ref(idx+1, 2);
end
end
function [c, ceq] = nonlincon(Z, TestTrack, Xobs)
function [c, ceq] = road_obstacle_cons(Z, TestTrack, Xobs)
global num_preds num_states
ceq = [];
@@ -255,10 +255,11 @@ function [c, ceq] = nonlincon(Z, TestTrack, Xobs)
[~,bl_idx] = min(vecnorm(TestTrack.bl - p));
[~,br_idx] = min(vecnorm(TestTrack.br - p));
bl_idx_start = clamp(bl_idx-1, 1, size(TestTrack.bl,2));
bl_idx_end = clamp(bl_idx+1, 1, size(TestTrack.bl,2));
br_idx_start = clamp(br_idx-1, 1, size(TestTrack.br,2));
br_idx_end = clamp(br_idx+1, 1, size(TestTrack.br,2));
idx_search = 1;
bl_idx_start = clamp(bl_idx-idx_search, 1, size(TestTrack.bl,2));
bl_idx_end = clamp(bl_idx+idx_search, 1, size(TestTrack.bl,2));
br_idx_start = clamp(br_idx-idx_search, 1, size(TestTrack.br,2));
br_idx_end = clamp(br_idx+idx_search, 1, size(TestTrack.br,2));
boundary_pts = [ ...
TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...