mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 17:22:45 +00:00
Incorporate xenia-nonlinear branch changes
- Copy-paste `xenia_nonlinearopt.m` from xenia-nonlinear branch
This commit is contained in:
Sravan Balaji
@@ -1,3 +1,4 @@
|
||||
%% initial conditions
|
||||
%Vehicle Parameterrs
|
||||
Nw=2;
|
||||
f=0.01;
|
||||
@@ -30,131 +31,126 @@ curr_pos = [init(1);init(3)];
|
||||
% [g,h,dg,dh]=nonlcon(z, Xobs, nsteps);
|
||||
% [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps);
|
||||
|
||||
%%okay, idea is to create an iterative process that uses the centerline as a
|
||||
%checkpoint
|
||||
%keep trajecting until closer to the next point on centerline, then repeat
|
||||
%bounds will be based on index of current&previous cp index
|
||||
%cost function uses next checkpoint in centerline
|
||||
|
||||
%generate z and make it bigger if didn't reach close enough to the goal
|
||||
%(modify nsteps)
|
||||
Nobs = randi([10 25], 1,1);
|
||||
global Xobs
|
||||
Xobs = generateRandomObstacles(Nobs);
|
||||
|
||||
% global index_cl_cp %index of current checkpoint in centerline
|
||||
% index_cl_cp = 1;
|
||||
% global index_cl_nextcp %index of next checkpoint in centerline
|
||||
% index_cl_nextcp = 2;
|
||||
%
|
||||
% global nsteps
|
||||
% nsteps = 100;
|
||||
|
||||
% x0 = init;
|
||||
% x0vec = [];
|
||||
% %initialize x0vec to be initial condition for n steps and Sravan's ref inputs
|
||||
% %note: function 'initvec' changes inputs, but i'm not sure how
|
||||
% %initialization should look with fmincon
|
||||
% for i = 1:nsteps
|
||||
% x0vec = [x0vec, init];
|
||||
% end
|
||||
% for i = 1:nsteps-1
|
||||
% x0vec = [x0vec, -0.004, 3900];
|
||||
% end
|
||||
|
||||
% dist12atcp = [];
|
||||
% iter = 0;
|
||||
%
|
||||
% factor = 0.1; %next checkpoint if significantly closer to next checkpoint
|
||||
U_final = [];
|
||||
Y_final = [];
|
||||
options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
|
||||
'SpecifyObjectiveGradient',true) ;
|
||||
|
||||
load('ROB535_ControlProject_part1_Team3.mat');
|
||||
load('ROB535_ControlProject_part1_Team3.mat');
|
||||
|
||||
%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init);
|
||||
load('reftrack_info.mat');
|
||||
|
||||
load('segments_info.mat');
|
||||
|
||||
for i = 1:length(num_pts)
|
||||
[start_idx, end_idx] = get_indices(i, num_pts);
|
||||
%% MPC
|
||||
U_ref = ROB535_ControlProject_part1_input';
|
||||
Y_ref = Y_submission';
|
||||
dt = 0.01;
|
||||
|
||||
%discretize dynamics
|
||||
F_zf=b/(a+b)*m*g;
|
||||
F_zr=a/(a+b)*m*g;
|
||||
|
||||
%we are just going to use cornering stiffness to make linear so this derivative
|
||||
%easier, the vehicle parameter's are close enough to problem 1 hw 2
|
||||
B=10;
|
||||
C=1.3;
|
||||
D=1;
|
||||
Ca_r= F_zr*B*C*D;
|
||||
Ca_f= F_zf*B*C*D;
|
||||
|
||||
x = @(s,i) Y_ref(s,i);
|
||||
|
||||
Ac = @(i) [0, cos(x(5,i)), 0, -sin(x(5,i)), x(2,i)*sin(x(5,i))-x(4,i)*cos(x(5,i)), 0;
|
||||
0, (-1/m)*Ca_f*x(2,i)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
|
||||
0, sin(x(5,i)), 0, cos(x(5,i)), -x(4,i)*sin(x(5,i))+x(2,i)*cos(x(5,i)), 0;
|
||||
0, (1/m)*(-Ca_f*x(2,i)^-2 - Ca_r*x(2,i)^-2) - 1, 0, Ca_r/m*(-1/x(2,i)) + Ca_f/m*(-1/x(2,i)), 0, Ca_r/m*(b/x(2,i)) + Ca_f/m*(-a/x(2,i)) - x(2,i);
|
||||
0, 0, 0, 0, 0, 1
|
||||
0, (1/Iz)*(-Ca_f*a*x(2,i)^-2 - b*Ca_r*x(2,i)^-2), 0, -b*Ca_r/Iz*(-1/x(2,i)) + a*Ca_f/Iz*(-1/x(2,i)), 0, -b*Ca_r/Iz*(b/x(2,i)) + a*Ca_f/Iz*(-a/x(2,i))];
|
||||
|
||||
Bc = @(i) [0, -Ca_f/m, 0, Ca_f/m, 0, a*Ca_f/Iz;
|
||||
0, Nw/m, 0, 0, 0, 0]';
|
||||
|
||||
A = @(i) eye(6) + dt*Ac(i);
|
||||
B = @(i) dt*Bc(i);
|
||||
|
||||
|
||||
%Decision variables
|
||||
npred = 10;
|
||||
nstates = 6;
|
||||
ninputs = 2;
|
||||
Ndec=(npred+1)*nstates+ninputs*npred;
|
||||
|
||||
input_range = [-0.5, 0.5; -5000, 5000];
|
||||
eY0 = init';
|
||||
[Aeq_test1,beq_test1]=eq_cons(1,A,B,eY0,npred,nstates,ninputs);
|
||||
%simulate forward
|
||||
T = 0:0.01:(size(Y_ref,2)/100);
|
||||
%final trajectory
|
||||
Y=NaN(nstates,length(T));
|
||||
|
||||
%applied inputs
|
||||
U=NaN(ninputs,length(T));
|
||||
|
||||
%input from QP
|
||||
u_mpc=NaN(ninputs,length(T));
|
||||
|
||||
%error in states (actual-reference)
|
||||
eY=NaN(nstates,length(T));
|
||||
|
||||
%set random initial condition
|
||||
Y(:,1)=eY0+Y_ref(:,1);
|
||||
|
||||
for i=1:length(T)-1
|
||||
i
|
||||
%shorten prediction horizon if we are at the end of trajectory
|
||||
npred_i=min([npred,length(T)-i]);
|
||||
|
||||
delta = delta_vals(i);
|
||||
F_x = F_x_vals(i);
|
||||
%calculate error
|
||||
eY(:,i)=Y(:,i)-Y_ref(:,i);
|
||||
|
||||
%generate equality constraints
|
||||
[Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs);
|
||||
|
||||
if (end_idx >= size(Y_submission,1))
|
||||
start_idx = start_idx - 1;
|
||||
end_idx = end_idx - 1;
|
||||
end
|
||||
%generate boundary constraints
|
||||
[Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs);
|
||||
|
||||
x0 = [];
|
||||
for j = start_idx:end_idx+1 %+1 end idx to keep z size consistent to hw
|
||||
x0 = [x0, Y_submission(j,:)];
|
||||
end
|
||||
%cost for states
|
||||
Q=[1,1,1,1,1,1];
|
||||
|
||||
for j = start_idx:end_idx
|
||||
x0 = [x0, delta, F_x];
|
||||
end
|
||||
%cost for inputs
|
||||
R=[0.1,0.01];
|
||||
|
||||
%start and end index used to maintain size of vectors
|
||||
[lb, ub] = bounds(start_idx, end_idx);
|
||||
H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);
|
||||
|
||||
%define for cost function, goal is to reach end of segment
|
||||
global target_vec
|
||||
target_vec = [Y_submission(end_idx,1), Y_submission(end_idx,3), Y_submission(end_idx,5)];
|
||||
f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1);
|
||||
|
||||
global nsteps
|
||||
nsteps = num_pts(i)+1;
|
||||
[x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub);
|
||||
|
||||
cf=@costfun
|
||||
nc=@nonlcon
|
||||
z=fmincon(cf,x0,[],[],[],[],lb',ub',nc,options);
|
||||
Y0=reshape(z(1:6*nsteps),6,nsteps)';
|
||||
U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
|
||||
info = getTrajectoryInfo(Y0,U)
|
||||
U_final = [U_final; U];
|
||||
%get linearized input
|
||||
u_mpc(:,i)=x(nstates*(npred_i+1)+1:nstates*(npred_i+1)+ninputs);
|
||||
|
||||
%get input
|
||||
U(:,i)=u_mpc(:,i)+U_ref(:,i);
|
||||
|
||||
|
||||
%simulate model
|
||||
[~,ztemp]=ode45(@(t,z)kinematic_bike(t,z,U(:,i),0),[0 dt],Y(:,i));
|
||||
|
||||
%store final state
|
||||
Y(:,i+1)=ztemp(end,:)';
|
||||
end
|
||||
% while (index_cl_cp < size(TestTrack.cline,2))
|
||||
% %because of the way cf/nc need to be)
|
||||
% index_cl_cp
|
||||
% iter = iter + 1;
|
||||
% [lb, ub] = bounds(nsteps, index_cl_cp);
|
||||
% cf=@costfun
|
||||
% nc=@nonlcon
|
||||
% z=fmincon(cf,x0vec,[],[],[],[],lb',ub',nc,options);
|
||||
% Y0=reshape(z(1:6*nsteps),6,nsteps)';
|
||||
% U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
|
||||
%
|
||||
% [Y, T] = forwardIntegrateControlInput(U, x0vec(1:6));
|
||||
%
|
||||
% curr_xy = [Y(end,1); Y(end,3)];
|
||||
% dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
|
||||
% dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
|
||||
%
|
||||
% if (dist2cp2 < (dist2cp1 - dist2cp1*factor))
|
||||
% dist12atcp = [dist12atcp; dist2cp1, dist2cp2];
|
||||
% %add to final solution
|
||||
% U_final = [U_final; U];
|
||||
% Y_final = [Y_final; Y];
|
||||
%
|
||||
% %reinstantiate
|
||||
% %nsteps = 100;
|
||||
% x0vec = initvec(Y_final(end,:), U_final(end,:));
|
||||
%
|
||||
% %update checkpoint
|
||||
% index_cl_cp = index_cl_cp + 1
|
||||
% index_cl_nextcp = index_cl_nextcp + 1;
|
||||
% if (index_cl_nextcp > size(TestTrack.cline, 2))
|
||||
% index_cl_nextcp = size(TestTrack.cline, 2);
|
||||
% end
|
||||
% else
|
||||
% %resize and try again
|
||||
% %nsteps = nsteps + 20;
|
||||
% x0vec = initvec(x0vec(1:6), U(1,:));
|
||||
%
|
||||
% end
|
||||
%
|
||||
% end
|
||||
|
||||
|
||||
%% function start
|
||||
|
||||
function [start_idx, end_idx] = get_indices(segment_num, num_pts)
|
||||
if segment_num == 1
|
||||
@@ -166,6 +162,7 @@ function [start_idx, end_idx] = get_indices(segment_num, num_pts)
|
||||
end
|
||||
end
|
||||
|
||||
%not used
|
||||
function x0vec = initvec(x0, u0)
|
||||
%function used because fmincon needs initial condition to be size of
|
||||
%state vector
|
||||
@@ -184,6 +181,115 @@ function x0vec = initvec(x0, u0)
|
||||
end
|
||||
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
|
||||
|
||||
%initial condition
|
||||
x_initial=x_initial(:);
|
||||
|
||||
%size of decision variable and size of part holding states
|
||||
zsize=(npred+1)*nstates+npred*ninputs;
|
||||
xsize=(npred+1)*nstates;
|
||||
|
||||
Aeq=zeros(xsize,zsize);
|
||||
Aeq(1:nstates,1:nstates)=eye(nstates); %initial condition
|
||||
beq=zeros(xsize,1);
|
||||
beq(1:nstates)=x_initial;
|
||||
|
||||
state_idxs=nstates+1:nstates:xsize;
|
||||
input_idxs=xsize+1:ninputs:zsize;
|
||||
|
||||
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);
|
||||
|
||||
%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);
|
||||
end
|
||||
end
|
||||
|
||||
function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs)
|
||||
%time_idx is the index along uref the initial condition is at
|
||||
xsize=(npred+1)*nstates;
|
||||
usize=npred*ninputs;
|
||||
|
||||
Lb=[];
|
||||
Ub=[];
|
||||
for j=1:ninputs
|
||||
Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
|
||||
Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
|
||||
end
|
||||
|
||||
Lb=reshape(Lb,[usize,1]);
|
||||
Ub=reshape(Ub,[usize,1]);
|
||||
|
||||
Lb=[-Inf(xsize,1);Lb];
|
||||
Ub=[Inf(xsize,1);Ub];
|
||||
|
||||
end
|
||||
|
||||
function dzdt=kinematic_bike(t,x,U,T)
|
||||
%constants
|
||||
Nw=2;
|
||||
f=0.01;
|
||||
Iz=2667;
|
||||
a=1.35;
|
||||
b=1.45;
|
||||
By=0.27;
|
||||
Cy=1.2;
|
||||
Dy=0.7;
|
||||
Ey=-1.6;
|
||||
Shy=0;
|
||||
Svy=0;
|
||||
m=1400;
|
||||
g=9.806;
|
||||
|
||||
|
||||
%generate input functions
|
||||
delta_f=U(1);
|
||||
F_x=U(2);
|
||||
|
||||
%slip angle functions in degrees
|
||||
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
|
||||
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
|
||||
|
||||
%Nonlinear Tire Dynamics
|
||||
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
|
||||
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
|
||||
|
||||
F_zf=b/(a+b)*m*g;
|
||||
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
|
||||
|
||||
F_zr=a/(a+b)*m*g;
|
||||
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
|
||||
|
||||
F_total=sqrt((Nw*F_x)^2+(F_yr^2));
|
||||
F_max=0.7*m*g;
|
||||
|
||||
if F_total>F_max
|
||||
|
||||
F_x=F_max/F_total*F_x;
|
||||
|
||||
F_yr=F_max/F_total*F_yr;
|
||||
end
|
||||
|
||||
%vehicle dynamics
|
||||
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
|
||||
(-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
|
||||
x(2)*sin(x(5))+x(4)*cos(x(5));...
|
||||
(F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
|
||||
x(6);...
|
||||
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
|
||||
end
|
||||
|
||||
%% nonlinear opt functions
|
||||
function [lb, ub] = bounds(start_idx, end_idx)
|
||||
load('TestTrack.mat');
|
||||
load('reftrack_info.mat');
|
||||
@@ -361,6 +467,8 @@ function [g, h, dg, dh] = nonlcon(z)
|
||||
|
||||
end
|
||||
|
||||
%% linearization functions
|
||||
|
||||
function dzdt=bike(x,U)
|
||||
%constants
|
||||
Nw=2;
|
||||
|
||||
Reference in New Issue
Block a user