From 79711d6e7ad171c05190e033b5b68dfc879e37c7 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Thu, 9 Dec 2021 13:02:37 -0500 Subject: [PATCH] Start Implementing MPC based on Xenia's work - Copy `xenia_nonlinearopt.m` to `xenia_sravan_nonlinearopt.m` - Clean up nonlinear bike model: add comments, whitespace, intermediate variables, etc. - Add helper function `clamp` to clamp a value between min and max limits --- Experimentation/xenia_sravan_nonlinearopt.m | 580 ++++++++++++++++++++ 1 file changed, 580 insertions(+) create mode 100644 Experimentation/xenia_sravan_nonlinearopt.m diff --git a/Experimentation/xenia_sravan_nonlinearopt.m b/Experimentation/xenia_sravan_nonlinearopt.m new file mode 100644 index 0000000..c49bb05 --- /dev/null +++ b/Experimentation/xenia_sravan_nonlinearopt.m @@ -0,0 +1,580 @@ +%% System Parameters +% Vehicle Parameters (Table 1) +global Nw f Iz a b By Cy Dy Ey Shy Svy m g +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; + +% Input Limits (Table 1) +global delta_lims Fx_lims +delta_lims = [-0.5, 0.5]; +Fx_lims = [-5e3, 5e3]; + +% Initial Conditions (Equation 15) +state_init = [ ... + 287; ... % x [m] + 5; ... % u [m/s] + -176; ... % y [m] + 0; ... % v [m/s] + 2; ... % psi [rad] + 0; ... % r [rad/s] +]; + +%% Load Files +load("TestTrack.mat"); + +%% Setup +curr_pos = [state_init(1);state_init(3)]; + +%state_size = tbd; +% z = [init, init, -0.004, 3900]; %for testing purposes +% nsteps = 2; +% [LB, UB] = bounds(nsteps, 1); +% [g,h,dg,dh]=nonlcon(z, Xobs, nsteps); +% [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps); + + +Nobs = randi([10 25], 1,1); +global Xobs +Xobs = generateRandomObstacles(Nobs); + + + +U_final = []; +Y_final = []; +options = optimoptions('fmincon','SpecifyConstraintGradient',true,... + 'SpecifyObjectiveGradient',true) ; + +load('ROB535_ControlProject_part1_Team3.mat'); + +%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init); +load('reftrack_info.mat'); + +load('segments_info.mat'); + +%% 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 = state_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]); + + %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); + + %generate boundary constraints + [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs); + + %cost for states + Q=[1,1,1,1,1,1]; + + %cost for inputs + R=[0.1,0.01]; + + 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); + + %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 + + +%% function start + +function [start_idx, end_idx] = get_indices(segment_num, num_pts) + if segment_num == 1 + start_idx = 1; + end_idx = num_pts(segment_num); + else + start_idx = sum(num_pts(1:segment_num-1)) + 1; + end_idx = sum(num_pts(1:segment_num)); + end +end + +%not used +function x0vec = initvec(x0, u0) + %function used because fmincon needs initial condition to be size of + %state vector + %x0 - last row of Y at checkpoint + %u0 - last row of U at checkpoint + global nsteps + x0vec = []; + for i = 1:nsteps + x0vec = [x0vec, x0]; + end + + %not sure if inputs should be instantiated or not + %will instantiate them to previous u + for i = 1:nsteps-1 + x0vec = [x0vec, 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) + global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims + + %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'); + + %[m,nPts] = size(TestTrack.cline); +% numState = 6; +% numInput = 2; + % nsteps = StepsPerPoint * nPts; + + lb_u = [-0.5;-5000]; + ub_u = [0.5;5000]; + + bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ... + TestTrack.br(1,1), TestTrack.br(1,2)]; + bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ... + TestTrack.br(2,1), TestTrack.br(2,2)]; + phi_init = TestTrack.theta(1); + + %phi restricted to just [-pi pi] + %lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf]; + %ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf]; + lb = []; ub = []; + + %hijacking this for loop to only consider one index for bounds + for i=start_idx:end_idx+1 + Y_ref_curxy = [Y_submission(i,1); Y_submission(i,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); -Inf; min(bound_Y); -Inf; -pi; -Inf]; + ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf]; + + + lb=[lb;lb_x]; + ub=[ub;ub_x]; + + end + + for i=start_idx:end_idx + ub=[ub;ub_u]; + lb=[lb;lb_u]; + end +end + +function [J, dJ] = costfun(z) + global nsteps + global target_vec + load('TestTrack.mat'); + + targetPos = target_vec(1:2); + targetTheta = target_vec(3); + sumPos = []; + sumInput = []; + for i = 1:nsteps + zIndx = 6*(i-1) + 1; + sumPos(i) = (z(zIndx) - targetPos(1))^2 + (z(zIndx+2) - targetPos(2))^2 + (z(zIndx+4) - targetTheta)^2; + + if (i <= nsteps-1) + uInd = 2*(i-1) + nsteps*6 - 1; + sumInput(i) = z(uInd)^2 + z(uInd+1)^2; + end + end + + J = sum(sumPos) + sum(sumInput); + dJ = transpose(z.*2); + + uStart = nsteps*6 - 1; + for i = 1:6:nsteps*6 + zIndx = i; + dJ(i) = (z(zIndx) - targetPos(1))*2; + dJ(i+2) = (z(zIndx+2) - targetPos(2))*2; + dJ(i+4) = (z(zIndx+4) - targetTheta)*2; + end +end + +function [g, h, dg, dh] = nonlcon(z) + %nsteps = (size(z,2)/8); + global nsteps + global Xobs + curr_pos = [z(1); z(3)]; + + Xobs_seen = senseObstacles(curr_pos, Xobs); + centroids = []; + for i = 1:size(Xobs_seen,2) + centroids = [centroids; mean(Xobs_seen{1, i})]; + end + + dt = 0.01; + g = []; dg = []; + + %radius size + r = 3; + + for i = 1:nsteps + zInd_x = 6*(i-1) + 1; + zInd_y = 6*(i-1) + 3; + curr_xy = [z(zInd_x), z(zInd_y)]; + + %g based on if there's obstacles around + %initialize to zero + g_curr = 0; + zeroRow = zeros(1,size(z,2)); + dg(i,:) = zeroRow; + if (~isempty(centroids)) + dist2Obst = []; + for j = 1:size(Xobs_seen,2) + dist = norm(curr_xy(1) - centroids(j,1)) + norm(curr_xy(2) - centroids(j,2)); + dist2Obst = [dist2Obst; dist]; + end + + %closest obstacle used for constraint + [minDist, indMin] = min(dist2Obst); + + %g + g_curr = r^2 - (curr_xy(1) - centroids(indMin, 1))^2 - (curr_xy(2) - centroids(indMin, 2))^2; + + %dg + dg(i,zInd_x) = curr_xy(1)*(-2) - centroids(indMin, 1)*2; + dg(i,zInd_y) = curr_xy(2)*(-2) - centroids(indMin, 2)*2; + end + g = [g; g_curr]; + end + dg = transpose(dg); + + h = z(1:6); + for i = 2:nsteps + zInd_x = 6*(i-1) + 1; + zInd_x_prev = 6*(i-2) + 1; + + stateCurr = z(zInd_x:zInd_x+5); + statePrev = z(zInd_x_prev:zInd_x_prev+5); + + %get previous input + uInd_prev = 2*(i-1) + nsteps*6 - 1; + uPrev = [z(uInd_prev), z(uInd_prev + 1)]; + + derPrev= dt*bike(statePrev, uPrev); + + currH = stateCurr - statePrev - derPrev; + h(6*(i-1)+1) = currH(1); + h(6*(i-1)+2) = currH(2); + h(6*(i-1)+3) = currH(3); + h(6*(i-1)+4) = currH(4); + h(6*(i-1)+5) = currH(5); + h(6*(i-1)+6) = currH(6); + end + + dh = zeros(size(z,2), nsteps*6); + dh(1:6, 1:6) = eye(6); + for i = 1:nsteps-1 + uInd = 2*(i-1) + nsteps*6 + 1; + uCurr = [z(uInd), z(uInd + 1)]; + zInd_x = 6*(i-1) + 1; + stateCurr = z(zInd_x:zInd_x+5); + [A, B] = bikeLinearize(stateCurr, uCurr); + + dh(6*i+1:6*i+6, 6*i+1:6*i+6) = eye(6); + dh(6*i-5:6*i, 6*i+1:6*i+6) = -eye(6) - dt*A; + dh(nsteps*6+2*i-1:nsteps*6+2*i, 6*i+1:6*i+6) = -dt*transpose(B); + end + +end + +%% Linearization Functions +function dXdt = nonlinear_bike_model(X,U) + global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims + + % Get state & input variables + x = X(1); + u = X(2); + y = X(3); + v = X(4); + psi = X(5); + r = X(6); + + delta_f = U(1); + F_x = U(2); + + % Front and rear lateral slip angles in radians (Equations 8 & 9) + alpha_f_rad = delta_f - atan2(v + a*r, u); + alpha_r_rad = -atan2(v - b*r, u); + + % Convert radians to degrees for other equations + alpha_f = rad2deg(alpha_f_rad); + alpha_r = rad2deg(alpha_r_rad); + + % Nonlinear Tire Dynamics (Equations 6 & 7) + phi_yf = (1-Ey)*(alpha_f + Shy) + (Ey/By)*atan(By*(alpha_f + Shy)); + phi_yr = (1-Ey)*(alpha_r + Shy) + (Ey/By)*atan(By*(alpha_r + Shy)); + + % Lateral forces using Pacejka "Magic Formula" (Equations 2 - 5) + 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; + + % Limits on combined longitudinal and lateral loading of tires + % (Equations 10 - 14) + 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 + + % Apply input limits (Table 1) + delta_f = clamp(delta_f, delta_lims(1), delta_lims(2)); + F_x = clamp(F_x, Fx_lims(1), Fx_lims(2)); + + % Vehicle Dynamics (Equation 1) + dx = u*cos(psi) - v*sin(psi); + du = (1/m)*(-f*m*g + Nw*F_x - F_yf*sin(delta_f)) + v*r; + dy = u*sin(psi) + v*cos(psi); + dv = (1/m)*(F_yf*cos(delta_f) + F_yr) - u*r; + dpsi = r; + dr = (1/Iz)*(a*F_yf*cos(delta_f) - b*F_yr); + dXdt = [dx; du; dy; dv; dpsi; dr]; +end + +function [A, B] =bikeLinearize(x,U) + global Nw f Iz a b By Cy Dy Ey Shy Svy m g delta_lims Fx_lims + + %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 + + %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; + + A = [0, cos(x(5)), 0, -sin(x(5)), x(2)*sin(x(5))-x(4)*cos(x(5)), 0; + 0, (-1/m)*Ca_f*x(2)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1; + 0, sin(x(5)), 0, cos(x(5)), -x(4)*sin(x(5))+x(2)*cos(x(5)), 0; + 0, (1/m)*(-Ca_f*x(2)^-2 - Ca_r*x(2)^-2) - 1, 0, Ca_r/m*(-1/x(2)) + Ca_f/m*(-1/x(2)), 0, Ca_r/m*(b/x(2)) + Ca_f/m*(-a/x(2)) - x(2); + 0, 0, 0, 0, 0, 1 + 0, (1/Iz)*(-Ca_f*a*x(2)^-2 - b*Ca_r*x(2)^-2), 0, -b*Ca_r/Iz*(-1/x(2)) + a*Ca_f/Iz*(-1/x(2)), 0, -b*Ca_r/Iz*(b/x(2)) + a*Ca_f/Iz*(-a/x(2))]; + + B = [0, -Ca_f/(x(2)*m), 0, Ca_f/m, 0, a*Ca_f/Iz; + 0, Nw/m, 0, 0, 0, 0]'; +end + +%% Helper Functions +function clamped_val = clamp(val, min, max) + clamped_val = val; + + if clamped_val < min + clamped_val = min; + elseif clamped_val > max + clamped_val = max; + end +end \ No newline at end of file