clc;清除所有;关闭所有;%% paramtri %% Inercija;{公斤。m2} Juc = 0.026;%2.6e-2;Jlc = 0.001;%1e-3;Jm = 1.5e-3;% % Krutost;{N。m/rad} kuc = 12; %12; klc = 2; %2; %% Prigusenje; {N.m.s/rad} duc = 0.06; %6e-2; dlc = 6; dm = 6e-2; %% Radijus; {m} rpin = 7.78e-1; %7.78e2; %% Sila; {N} Fr = 0.1; %100; %% Masa; {kg} mr = 20; % %% Moment; {N.m} % MsAmp = 4.5; % % %% Kutna brzina; {rad/s} % freq = 2; %% Konstanta elektromotora k = 0.015; %% freq = 3; % time = 0: 0.01: 0.2; t2 = 0: 0.01: 3; MsAmp = 4.5; Ms = MsAmp*sin(2*pi*freq*t2); if Ms >=3.5 I = 21.29 * Ms - 69.4; elseif Ms >= 2 I = 2.73 * Ms - 4.47; elseif Ms >= 0 I = 0.5 * Ms; elseif Ms >= (-2) I = 0.5 * Ms; elseif Ms >= (-3.5) I = 2.73 * Ms + 4.47; elseif Ms < (-3.5) I = 21.29 * Ms + 69.4; end %% Matrice A, B, C i D A = [0 1 0 0 0 0 0 0 0; -(kuc/Juc) -(duc/Juc) (kuc/Juc) (duc/Juc) 0 0 0 0 0; 0 0 0 1 0 0 0 0 0; (kuc/Jlc) (duc/Jlc) -(kuc/Jlc) -(duc/Jlc) -(klc/Jlc) -(dlc/Jlc) (klc/(Jlc*rpin)) (dlc/(Jlc*rpin)) 0; 0 0 0 0 0 1 0 0 0; 0 0 0 0 0 0 0 1 0; 0 0 0 0 -(klc/(mr*rpin)) -(dlc/(mr*rpin)) (klc/(mr*rpin)) (dlc/(mr*rpin)) 0; 0 0 0 0 0 0 0 0 1; 0 0 0 0 0 0 0 0 -(dm/Jm)]; B = [0 0 0; 1 -k 0; 0 0 0; 0 (k/Jlc) 0; 0 0 0; 0 0 0; 0 0 1; 0 0 0; 0 (k/Jm) 0]; C = B.'; D = 0; %% Odziv sustava bez LQR % figure % sys = ss(A, B, C, D); % ss = prostor stanja % t1 = 0:0.1:5; % stepplot(sys,t1); % set(gcf,'color','w'); % xlabel('Vrijeme'); % ylabel('Odziv'); % hold on; % title('Odziv svakog izlaza u odnosu na ulaz bez LQR-a'); vlast_vrijed = eig(A); %jedan clan je pozitivan matrica_upravljivosti = ctrb(A,B); upravljivost = rank(matrica_upravljivosti); %% LQR = lqr(A,B,Q,R); Q = 10*eye(9); R = 1*eye(3); K = lqr(A,B,Q,R); nova_matrica_upravljivosti = A-B*K; nove_vlast_vrijed = eig(A-B*K); %sada cu svi clanovi negativni % figure % sysss = ss(nova_matrica_upravljivosti, B, C, D); % stepplot(sysss, t1); % set(gcf,'color','w'); % xlabel('Vrijeme'); % ylabel('Odziv'); % hold on; % title('Odziv svakog izlaza u odnosu na ulaz sa LQR-om'); %% SVD dekompozicija [U,S,V] = svd(A); %% Prikaz odziva svih varijabli tspan = 0: .001: 3; x0 = zeros(9,1); [t,x] = ode113(@(t,x) epasFun(t,x,Juc,Jlc,kuc,klc,duc,dlc,rpin,mr,dm,Jm,k,Fr), tspan, x0); %% %[t,x] = ode113(@(t,x) epasFun(tspan,x,Juc,Jlc,kuc,klc,duc,dlc,rpin,mr,dm,Jm,k,Fr), tspan, x0); %% % X = zeros(numel(time),numel(tspan),9); %for i = 1:numel(time) % [t,x] = ode45(@(t,x) epasFun(x,Juc,Jlc,kuc,klc,duc,dlc,rpin,mr,dm,Jm,k,Ms(i),I(i),Fr), tspan, x0); % X(i,:,:)=x(:,:); %end %figure %plot(t,squeeze(X(:,:,8))); % set(gcf,'color','w'); %x = X; %% figure plot(t,x(:,1),'Linewidth', 1); hold on; plot(t,x(:,2),'Linewidth', 1); hold on; plot(t,x(:,3),'Linewidth', 1); hold on; plot(t,x(:,4),'Linewidth', 1); hold on; plot(t,x(:,5),'Linewidth', 1); hold on; plot(t,x(:,6),'Linewidth', 1); hold on; plot(t,x(:,7),'Linewidth', 1); hold on; plot(t,x(:,8),'Linewidth', 1); hold on; plot(t,x(:,9),'Linewidth', 1); legend('deltaS', 'omegaS', 'deltaUC', 'omegaUC', 'deltaLC', 'sR', 'vR', 'deltaM', 'omegaM'); set(gcf,'color','w'); xlabel('Vrijeme'); ylabel('Odziv varijabli'); %% Odziv po varijablama figure subplot(3,3,1); plot(t,x(:,1),'Linewidth', 1); title('Kutna brzina upravljaca'); xlabel('[s]'); ylabel('[rad/s]'); set(gcf,'color','w'); subplot(3,3,2); plot(t,x(:,2),'Linewidth', 1); title('Kutno ubrzanje upravljaca'); xlabel('[s]'); ylabel('[rad/s^2]'); set(gcf,'color','w'); subplot(3,3,3); plot(t,x(:,3),'Linewidth', 1); title('Kutna brzina gornjeg dijela vratila'); xlabel('[s]'); ylabel('[rad/s]'); set(gcf,'color','w'); subplot(3,3,4); plot(t,x(:,4),'Linewidth', 1); title('Kutno ubrzanje gornjeg dijela vratila'); xlabel('[s]'); ylabel('[rad/s^2]'); set(gcf,'color','w'); subplot(3,3,5); plot(t,x(:,5),'Linewidth', 1); title('Kutna brzina donjeg dijela vratila'); xlabel('[s]'); ylabel('[rad/s]'); set(gcf,'color','w'); subplot(3,3,6); plot(t,x(:,6),'Linewidth', 1); title('Brzina ozubljenje letve'); xlabel('[s]'); ylabel('[m/s]'); set(gcf,'color','w'); subplot(3,3,7); plot(t,x(:,7),'Linewidth', 1); title('Ubrzanje ozubljene letve'); xlabel('[s]'); ylabel('[m/s^2]'); set(gcf,'color','w'); subplot(3,3,8); plot(t,x(:,8),'Linewidth', 1); title('Kutna brzina elektromotora'); xlabel('[s]'); ylabel('[rad/s]'); set(gcf,'color','w'); subplot(3,3,9); plot(t,x(:,9),'Linewidth', 1); title('Kutno ubrzanje elektromotora'); xlabel('[s]'); ylabel('[rad/s^2]'); set(gcf,'color','w'); %% figure plot(t,x(:,1),'Linewidth', 1); hold on; plot(t2,Ms);