diff --git a/Experimentation/xenia_nonlinearopt.m b/Experimentation/xenia_nonlinearopt.m index 56d03ca..e81f27c 100644 --- a/Experimentation/xenia_nonlinearopt.m +++ b/Experimentation/xenia_nonlinearopt.m @@ -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;