From f27a3b82e3bdeed7dbef3a14dcc3e1d2dff486d3 Mon Sep 17 00:00:00 2001 From: xdemenchuk Date: Thu, 9 Dec 2021 15:38:11 -0500 Subject: [PATCH] MPC kinda working/nonlinear --- SimPkg_F21(student_ver)/xenia_nonlinearopt.m | 77 ++++++++++++++------ 1 file changed, 56 insertions(+), 21 deletions(-) diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m index c222eb0..88564e8 100644 --- a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m +++ b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m @@ -34,7 +34,7 @@ curr_pos = [init(1);init(3)]; Nobs = randi([10 25], 1,1); global Xobs -Xobs = generateRandomObstacles(Nobs); +%Xobs = generateRandomObstacles(Nobs); @@ -85,17 +85,14 @@ Bc = @(i) [0, 0; 0, 0; -(a*sin(U_ref(1,i))*(Svy - (Dy*b*g*m*sin(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By))))/(a + b)) - (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(1 + Ey/(By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1) - Ey))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz, 0]; - -test1 = Ac(1) -test2 = Bc(2) - + A = @(i) eye(6) + dt*Ac(i); B = @(i) dt*Bc(i); %Decision variables -npred = 10; +npred = 20; nstates = 6; ninputs = 2; Ndec=(npred+1)*nstates+ninputs*npred; @@ -132,19 +129,19 @@ for i=1:length(T)-1 [Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs); %generate boundary constraints - [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs); + [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs, Y(:,i), Y_ref(:,i)); %cost for states - Q=[10,1,10,1,5,1]; + Q=[100,1,100,1,50,1]; %cost for inputs - R=[10,1]; + R=[100,1]; H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]); f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1); - - [x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub); + options = optimoptions('quadprog','Display','iter'); + [x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub,[],options); %get linearized input u_mpc(:,i)=x(nstates*(npred_i+1)+1:nstates*(npred_i+1)+ninputs); @@ -158,6 +155,23 @@ for i=1:length(T)-1 %store final state Y(:,i+1)=ztemp(end,:)'; + + traj_info = getTrajectoryInfo(Y',U'); + +% if (~isempty(traj_info.left_track_position)) +% [lb, ub] = bounds(1, npred); +% global nsteps +% nsteps = npred; +% cf=@costfun +% nc=@nonlcon +% z=fmincon(cf,x,[],[],[],[],lb',ub',nc,options); +% Y0=reshape(z(1:6*nsteps),6,nsteps); +% U=reshape(z(6*nsteps+1:end),2,nsteps-1); +% Y_ref(:,i) = Y0(:,end); +% U_ref(:,i) = U(:,end); +% i = i - 1; +% end + end @@ -226,23 +240,42 @@ function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs) end end -function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs) +function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs,currpos, Y_ref) %time_idx is the index along uref the initial condition is at + load('TestTrack.mat'); 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)]; + for i=1:(npred+1) + Y_ref_curxy = [currpos(1); currpos(3)]; + sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2; + dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5; + [minDist, indMin] = min(dist_to_cl); + + prev_idx = max(indMin-1, 1); + next_idx = min(indMin, 246); + + bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ... + TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)]; + bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ... + TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)]; + %phi_init = TestTrack.theta(i); + + lb_x = [min(bound_X)-Y_ref(1); -Inf; min(bound_Y)-Y_ref(3); -Inf; -pi; -Inf]; + ub_x = [max(bound_X)-Y_ref(1); Inf; max(bound_Y)-Y_ref(3); Inf; +pi; Inf]; + + + Lb=[Lb;lb_x]; + Ub=[Ub;ub_x]; + + end + + for j=1:npred + Lb=[Lb;input_range(:,1)-U_ref(:,initial_idx+j-1)]; + Ub=[Ub;input_range(:,2)-U_ref(:,initial_idx+j-1)]; end - - Lb=reshape(Lb,[usize,1]); - Ub=reshape(Ub,[usize,1]); - - Lb=[-Inf(xsize,1);Lb]; - Ub=[Inf(xsize,1);Ub]; end @@ -330,6 +363,8 @@ function [lb, ub] = bounds(start_idx, end_idx) sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2; dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5; [minDist, indMin] = min(dist_to_cl); + global target_vec + target_vec = [TestTrack.cline(1,indMin), TestTrack.cline(2,indMin), TestTrack.theta(indMin)]; prev_idx = max(indMin-1, 1); next_idx = min(indMin, 246);