Điều khiển chuyển động tuyến tính sử dụng động cơ POLYSOLENOID có xét đến hiệu kính đầu cuối

̵ ̵ clear clc %% Thong so dong co t = 0.02; p = 1; phi = 0.035; m = 0.171; Rs = 10.5; Lsd = 1.4e-3; Lsq = Lsd; %% Gian doan hoa mo hinh va trang thai dau Ts = 0.01; A = [0 1;0 0]; B = [0 0;2*pi*p*phi/(m*t) -1/m]; dss1 = c2d(ss(A,B,[1,0],0),Ts); Ad = dss1.A; Bb1 = dss1.B; Bd = Bb1(:,1); Dd = Bb1(:,2); Ns = 30; dss2 = c2d(ss(A,B,[1,0],0),Ts/Ns); Ad1 = dss2.A; Bb2 = dss2.B; Bd1 = Bb2(:,1); Dd1 = Bb2(:,2); AI = [-Rs/Lsd 0;0 -Rs/Lsq]; BI = [1/Lsd 0;0 1/Lsq]; dss3 = c2d(ss(AI,BI,[1,0],0),Ts/Ns); AId = dss3.A; BId = dss3.B; zk = [-0.1;0]; idq = [0;0]; zsol = zk; zplot = zk; Iplot = idq; Uplot = [0;0]; usol = 0; d_isdr = 0; d_isqr = 0; I_isd = 0; I_isq = 0; isdr_old = idq(1); isqr_old = idq(2); %% Rang buoc D = 10; d_set = [-1:0.1:1]; Z0max = [0.002;1.5]; Z0min = [-0.002;-1.5]; Zmax = [0.1;6]; Zmin = [-0.1;-6]; Umin = -2; Umax = 2; %% Giai max MPC n = 40; Nw = 6; % horizonal prediction R = 0.1; Q = diag([1,1]); K = [-300 -3]; uI = [0;0]; % inner %% Current controller k11 = 400; k12 = 1200; k21 = 400; k22 = 1200; %% loop ddL = zeros(Nw,1); DD = zeros(2,Nw,Nw); BB = zeros(2,Nw,Nw); AAcon = zeros(4,Nw,Nw); BBcon = zeros(4,1,Nw); Damp = 3; file = load('disturbance.mat'); fd = file.fd; for i = 1:1:n u_pos = 0; DD = zeros(2,Nw,Nw); BB = zeros(2,Nw,Nw); AAcon = zeros(4,Nw,Nw); if (abs(zk(1)) < Damp*Z0max(1)) && (abs(zk(2)) < Z0max(2)) u_pos = K*zk; else u1 = 0; dL = zeros(Nw,1); J = 0; count = max(size(d_set))+1; for i0 = d_set dk = i0*D; d = dk*ones(Nw,1); DD(:,:,1) = [Dd,zeros(2,Nw-1)]; BB(:,:,1) = [Bd,zeros(2,Nw-1)]; AAcon(:,:,1) = [BB(:,:,1);-BB(:,:,1)]; for ii = 2:1:Nw DD(:,:,ii)= [Ad^(ii-1)*Dd,DD(:,1:Nw-1,ii-1)]; BB(:,:,ii)= [Ad^(ii-1)*Bd,BB(:,1:Nw-1,ii-1)]; AAcon(:,:,ii) = [BB(:,:,ii);-BB(:,:,ii)]; BBcon(:,:,ii-1) = [Zmax - Ad^(ii-1)*zk - DD(:,:,ii-1)*d; -Zmin + Ad^(ii-1)*zk + DD(:,:,ii-1)*d]; end BBcon(:,:,Nw) = [Z0max - Ad^Nw*zk - DD(:,:,Nw)*d; -Z0min + Ad^Nw*zk + DD(:,:,Nw)*d]; AAconu = [[1;-1],zeros(2,Nw-1)]; BBconu = [Umax;-Umin]; for ii = 2:1:Nw AAconu = [AAconu;[[1;-1],zeros(2,Nw-1)]]; BBconu = [BBconu;[Umax;-Umin]]; end Acon11 = AAconu; Bcon11 = BBconu; for ii = 1:1:Nw Acon11 = [AAcon(:,:,Nw-ii+1);Acon11]; Bcon11 = [BBcon(:,:,Nw-ii+1);Bcon11]; end Hu1 = R*eye(Nw); fu1 = zeros(1,Nw); Hd = zeros(Nw,Nw); Gd = zeros(2,Nw); for ii = 1:1:Nw-1 Hu1 = Hu1 + BB(:,:,ii)'*Q*BB(:,:,ii); fu1 = fu1 + 2*zk'*(Ad^ii)'*Q*BB(:,:,ii) + 2*d'*DD(:,:,ii)'*Q*BB(:,:,ii); Hd = Hd + DD(:,:,ii)'*Q*DD(:,:,ii); Gd = Gd + (Ad^ii)'*Q*DD(:,:,ii); end u = quadprog(2*Hu1,fu1,Acon11,Bcon11); u1 = [u1;u(1)]; dL = [dL, d]; J1 = d'*Hd*d + 2*zk'*Gd*d+u'*Hu1*u+fu1*u; J = [J,J1]; end u1 = u1(2:count); J = J(2:count); for i4 = 1:1:count if (J(i4) == max(J)) u_pos = u1(i4); ddL = [ddL,dL(:,i4)]; break; end end end %fd = D/2+randi(10*D/2,1)/10; %fd = D; isdr = 0; ddxr = 0; for ii = 1:1:Ns if (abs(zk(1)) < Damp*Z0max(1)) && (abs(zk(2)) < Z0max(2)) u_pos = K*zk; end isqr = u_pos + m*p*t*ddxr/(2*pi*phi); % sai lech e_isd = isdr - idq(1); e_isq = isqr - idq(2); % vi phan d_isdr = (isdr - isdr_old)/(Ts/Nw); d_isqr = isqr_old)/(Ts/Nw); % tich phan I_isd = I_isd+ e_isd; I_isq = I_isq+ e_isq; % tin hieu dieu khien dong uI1 = Lsd*(k11*e_isd + k12*I_isd); uI2 = Lsq*(k21*e_isq + k22*I_isq); uI = [uI1;uI2]; (isqr idq = AId*idq + BId*uI; Iplot =[Iplot,idq]; isqr_old = isqr; zk = Ad1*zk + Bd1*idq(2) + Dd1*fd(i); zplot = [zplot,zk]; udq = uI+(2*pi*zk(2)*p/t)*[-Lsq*idq(2);Lsd*idq(1)+phi]; Uplot = [Uplot,udq]; end zsol = [zsol,zk]; usol = [usol,u_pos]; end zsol figure(1); plot(Ts*(0:1:Ns*n)/Ns,zplot(1,:)); grid on; hold on figure(2); plot(Ts*(0:1:Ns*n)/Ns,zplot(2,:)); grid on; hold on figure(3); plot(Ts*(0:1:Ns*n)/Ns,Iplot); grid on; hold on figure(4); plot(Ts*(0:1:Ns*n)/Ns,Uplot); grid on; hold on fdplot = zeros(1,2*n +2); time = zeros(1,2*n +2); for i = 2:2:2*n+2 fdplot(i) = fd(i/2); fdplot(i-1) = fdplot(i); time(i-1) = i/2; time(i) = time(i-1); end figure(5); plot(Ts*([time(2:2*n+2),n+1]-1),fdplot); grid on - clear all phiP = 0.035; Lsd = 1.4e-3; Lsq = 1.4e-3; R = 10.3; p = 1; m = 0.171; tau = 0.02; N = 1; Ts = 0.1e-3; % Mo hinh dong dien A = [-R/Lsd 0; -R/Lsq]; B = [1/Lsd 0; 1/Lsq]; M = [0 Lsq/Lsd; -Lsd/Lsq]; S = [0;-1/Lsq]; % Mo hinh dong dien dq Phi_dq1 = eye(2) + A*Ts; H_dq = Ts*B; h_dq = Ts*S; % Mo hinh toc va vi tri Tsw = 0.2e-2; Aw = [0 1;0 0]; Bw = [0 0;2*pi*p*phiP/(m*tau) -1/m]; dss = c2d(ss(Aw,Bw,[1,0],0),Tsw); Ad = dss.A; Bb1 = dss.B; Bd = Bb1(:,1); Dd = Bb1(:,2); % Bo dieu khien MinMax Nw = 6; K = [1000 10]; R = 0.01; Q = diag([10,0.01]); Z0max = [0.002;2]; Z0min = [-0.003;-2]; Zmax = [0.1;6]; Zmin = [-0.1;-6]; Umin = -2; Umax = 2; %MPC dong dien Q_MPC = diag([10 1]); Hu = H_dq'*Q_MPC*H_dq; [L,~] = chol(2*Hu,'lower'); Linv = inv(L); Acon = [1 1;1 -1;-1 1; -1 -1]; Bcon = Vdc*[1;1;1;1]; opt = mpcqpsolverOptions; opt.MaxIter = 100; opt.FeasibilityTol = 1.0e-3; iA0 = false(size(-Bcon)); Mag = 4*pi; Fre = 6; k1 = 800; k2 = 1; clear all phiP = 0.035; Lsd = 1.4e-3; Lsq = 1.4e-3; R = 10.3; p = 1; m = 0.171; tau = 0.02; N = 1; Ts = 0.05e-3; % Mo hinh dong dien A = [-R/Lsd 0; -R/Lsq]; B = [1/Lsd 0; 1/Lsq]; M = [0 Lsq/Lsd; -Lsd/Lsq]; S = [0;-1/Lsq]; % Mo hinh dong dien dq Phi_dq1 = eye(2) + A*Ts; H_dq = Ts*B; h_dq = Ts*S; % Mo hinh toc va vi tri Tsw = 0.2e-2; Aw = [0 1;0 0]; Bw = [0 0;2*pi*p*phiP/(m*tau) -1/m]; dss = c2d(ss(Aw,Bw,[1,0],0),Tsw); Ad = dss.A; Bb1 = dss.B; Bd = Bb1(:,1); Dd = Bb1(:,2); % Bo dieu khien MinMax Nw = 6; k1 = 800; k2 = 1; K = [k1 k2]; R = 0.01; Q = diag([10,0.01]); Z0max = [0.002;2]; Z0min = [-0.003;-2]; Zmax = [0.1;6]; Zmin = [-0.1;-6]; Umin = -2; Umax = 2; %MPC dong dien Q_MPC = diag([10 1]); Hu = H_dq'*Q_MPC*H_dq; Acon = [1 1;1 -1;-1 1; -1 -1]; Bcon = Vdc*[1;1;1;1]; Mag = 4*pi; Fre = 6; % Bang chuyen trang thai ung voi vector dien ap U = []; for i = pi/6:pi/6:2*pi U = [U, ((Vdc-2)/sqrt(2))*[sin(i); cos(i)]]; end UU = U; for j = 2:1:6 U = [U,(1/j)*UU]; end U = [U,[0;0]]; clear all phiP = 0.035; Lsd = 1.4e-3; Lsq = 1.4e-3; R = 10.3; p = 1; m = 0.171; tau = 0.02; Ts = 0.05e-3; % Mo hinh dong dien A = [-R/Lsd 0; -R/Lsq]; B = [1/Lsd 0; 1/Lsq]; M = [0 Lsq/Lsd; -Lsd/Lsq]; S = [0;-1/Lsq]; % Mo hinh dong dien dq Phi_dq1 = eye(2) + A*Ts; H_dq = Ts*B; h_dq = Ts*S; % Mo hinh toc va vi tri Tsw = 0.2e-2; Aw = [0 1;0 0]; Bw = [0 0;2*pi*p*phiP/(m*tau) -1/m]; dss = c2d(ss(Aw,Bw,[1,0],0),Tsw); Ad = dss.A; Bb1 = dss.B; Bd = Bb1(:,1); Dd = Bb1(:,2); % Bo dieu khien MinMax Nw = 6; k1 = 800; k2 = 1; K = [k1 k2]; R = 0.01; Q = diag([10,0.01]); Z0max = [0.002;2]; Z0min = [-0.003;-2]; Zmax = [0.1;6]; Zmin = [-0.1;-6]; Umin = -2; Umax = 2; Mag = 4*pi; Fre = 6; N = 2; l = ones(N,1)/N; function iq_ref = fcn(ex, ev, Ad, Bd, Dd, Nw, Z0max, Zmax, Umax, R, Q, acc, k1, k2) K = [k1 k2]; Zmin = -Zmax; Z0min = -Z0max; Umin = -Umax; %% loop DD = zeros(2,Nw,Nw); BB = zeros(2,Nw,Nw); AAcon = zeros(4,Nw,Nw); BBcon = zeros(4,1,Nw); zk = [ex;ev]; u_pos = 0; if (abs(zk(1)) < Z0max(1)) && (abs(zk(2)) < Z0max(2)) u_pos = -K*zk; else dk = [-15,0,15]; count = max(size(dk)); u1 = zeros(count,1); J = zeros(count,1); for i0 = 1:1:count d = dk(i0)*ones(Nw,1); BB(:,:,1) = [Bd,zeros(2,Nw-1)]; DD(:,:,1) = [Dd,zeros(2,Nw-1)]; AAcon(:,:,1) = [BB(:,:,1);-BB(:,:,1)]; for ii = 2:1:Nw DD(:,:,ii)= [Ad^(ii-1)*Dd,DD(:,1:Nw-1,ii-1)]; BB(:,:,ii)= [Ad^(ii-1)*Bd,BB(:,1:Nw-1,ii-1)]; AAcon(:,:,ii) = [BB(:,:,ii);-BB(:,:,ii)]; BBcon(:,:,ii-1) = [Zmax - Ad^(ii-1)*zk - DD(:,:,ii-1)*d; -Zmin + Ad^(ii-1)*zk + DD(:,:,ii-1)*d]; end BBcon(:,:,Nw) = [Z0max - Ad^Nw*zk - DD(:,:,Nw)*d; -Z0min + Ad^Nw*zk + DD(:,:,Nw)*d]; AAconu = zeros(2*Nw,Nw); AAconu(1:2,:) = [[1;-1],zeros(2,Nw-1)]; BBconu = zeros(2*Nw,1); BBconu(1:2,:) = [Umax;-Umin]; for ii = 2:1:Nw AAconu(2*ii-1:2*ii,:) = [[1;-1],zeros(2,Nw-1)]; BBconu(2*ii-1:2*ii,:) = [Umax;-Umin]; end AAA = zeros(4*Nw,Nw); BBB = zeros(4*Nw,1); for ii = 1:1:Nw AAA(4*ii-3:4*ii,:) = AAcon(:,:,ii); BBB(4*ii-3:4*ii,:) = BBcon(:,:,ii); end Acon11 = [AAA;AAconu]; Bcon11 = [BBB;BBconu]; Hu1 = R*eye(Nw); fu1 = zeros(1,Nw); Hd = zeros(Nw,Nw); Gd = zeros(2,Nw); for ii = 1:1:Nw-1 Hu1 = Hu1 + BB(:,:,ii)'*Q*BB(:,:,ii); fu1 = fu1 + 2*zk'*(Ad^ii)'*Q*BB(:,:,ii) + 2*d'*DD(:,:,ii)'*Q*BB(:,:,ii); Hd = Hd + DD(:,:,ii)'*Q*DD(:,:,ii); Gd = Gd + (Ad^ii)'*Q*DD(:,:,ii); end [u, status] = mpcCustomSolverCodeGen(2*Hu1,fu1,-Acon11,Bcon11); if(status == 0) u = zeros(Nw,1); end u1(i0) = u(1); J1 = d'*Hd*d + 2*zk'*Gd*d+u'*Hu1*u+fu1*u; J(i0) = J1; end for i4 = 1:1:count if (J(i4) == max(J)) u_pos = u1(i4); break; end end end iq_ref = u_pos + acc; clear all phiP=0.035; Lsd = 1.4e-3; Lsq = 1.4e-3; Rs = 10.3; p = 1; m = 0.171; tau = 0.02; b = 0.1; % he so ma sat N = 1; Ts = 0.1e-3; Ad1 = 0.2e-3; Bd1 = 0.2e-3; Aq1 = 0.2e-3; Bq1 = 0.2e-3; Adq1 = 0.1e-3; Bdq1 = 0.1e-3; para = [phiP m Rs Lsd Lsq Ad1 Bd1 Aq1 Bq1]; Fc = 0; % Mo hinh dong dien A = [-Rs/Lsd 0; -Rs/Lsq]; B = [1/Lsd 0; 1/Lsq]; M = [0 Lsq/Lsd; -Lsd/Lsq]; S = [0;-1/Lsq]; Mag = 4*pi; %tham so bo dieu khien beta = 0.01; Gamma = diag([500 10]); Pi = 0.1; k_theta = 50; k_omega = 0.01; L0 = [1e-4 1e-4 0 0 0]; function [Udq,d_upt] = f(id,iq,idc,iqc,didc,diqc,omega,theta,omega_c,upt,Pi, beta, Gamma, phiP, Rs) Y11 = - didc; Y12 = -omega*id; Y21 = omega*iq; Y22 = diqc; Y31 = -2*omega*cos(2*theta)*id sin(2*theta)*didc + omega*cos(2*theta)*(id - idc); Y32 = -omega*sin(2*theta)*id; Y41 = 2*omega*sin(2*theta)*id cos(2*theta)*didc omega*sin(2*theta)*(id - idc); Y42 = -omega*cos(2*theta)*id; Y51 = omega*sin(2*theta)*iq; Y52 = -2*omega*cos(2*theta)*iq sin(2*theta)*diqc + omega*cos(2*theta)*(iq - iqc); Y61 = omega*cos(2*theta)*iq; Y62 = 2*omega*sin(2*theta)*iq cos(2*theta)*diqc + omega*sin(2*theta)*(iq - iqc); Y71 = omega*(sin(2*theta)*id - 2*cos(2*theta)*iq) - sin(2*theta)*diqc + omega*cos(2*theta)*(iq - iqc); Y72 = -omega*(sin(2*theta)*iq + 2*cos(2*theta)*id) - sin(2*theta)*didc + omega*cos(2*theta)*(id - idc); Y81 = omega*(cos(2*theta)*id + 2*sin(2*theta)*iq) - cos(2*theta)*diqc omega*sin(2*theta)*(iq - iqc); Y82 = -omega*(cos(2*theta)*iq - 2*sin(2*theta)*id) - cos(2*theta)*didc - omega*sin(2*theta)*(id - idc); Y = [ Y11 Y12; Y21 Y22 ; Y31 Y32 ; Y41 Y42 ; Y51 Y52 ; Y61 Y62 ; Y71 Y72 ; Y81 Y82 ]'; G = [-Rs*id -Rs*iq-omega*phiP]; Udq = - G - [0;(omega-omega_c)] - Gamma*([id;iq] - [idc;iqc])-Y*upt; d_upt = Pi*(-beta*upt + Y'*[id-idc;iq-iqc]); function [idc,iqc,didc,diqc,omega_c] = fcn(theta_r,omega_r,a_r,da_r,theta,omega,iq, k_theta,phiP, tau, k_omega, p, m, Fc) tau_p = tau/p; a = (2*pi/tau_p)^2*(phiP/m); domega = a*iq - (2*pi/tau_p)*1/m*Fc; omega_c = omega_r - k_theta*(theta - theta_r); domega_c = a_r - k_theta*(omega - omega_r); ddomega_c = da_r - k_theta*(domega - a_r); idc = 0; didc = 0; iqc = -k_omega*(omega - omega_c) + 1/a*domega_c - (theta - theta_r) ; diqc = 0;