%%%%%%%% T MODELAGEM - PLATAFORMA ESTABILIZADORA %%%%%%%% %-------------------------------------------------------- % Andrei Steschenko % João Pedro Dias Nunes % Gabriel Jenner de Faria Orsi % Rogério Yukio Tamaoki Rodriguez %-------------------------------------------------------- %Comandos iniciais clc();clear();close(); %Definição dos vetores de tempo t0 = 0; %s tempo inicial t_f = 30; %s tempo final dt = 0.001; %s incremento de tempo t = 0:dt:t_f; %vetor de tempos ut = 0:dt:t_f; %vetor de tempos de entrada %------------------------------------------------------- %----------------SIMULAÇÃO DO MEIO CARRO---------------- %------------------------------------------------------- % % %MEIO CARRO LONGITUDINAL Ms=600; J=730; mwf=45; mwr=45; ksf=18000; ksr=18000; csf=500; csr=500; kwf=102017; kwr=102017; cwf=138; cwr=138; a=1.5; b=1.15; % % MEIO CARRO TRANSVERSAL % Ms=577.8; % J=108.3; % mwf=45; % mwr=45; % ksf=18000; % ksr=18000; % csf=500; % csr=500; % kwf=102017; % kwr=102017; % cwf=138; % cwr=138; % a=1.5; % b=1.15; %Condições iniciais da suspensão X0c=[0,0,0,0,0,0,0,0]; %Espaço de estados da suspensão Ac=[0 1 0 0 0 0 0 0 (-ksf-ksr)/Ms (-csf-csr)/Ms (ksf*a-ksr*b)/Ms (csf*a-csr*b)/Ms ksf/Ms csf/Ms ksr/Ms csr/Ms 0 0 0 1 0 0 0 0 (ksf*a-ksr*b)/J (csf*a-csr*b)/J (-ksf*a^2-ksr*b^2)/J (-csf*a^2-csr*b^2)/J -ksf*a/J -csf*a/J ksr*b/J csr*b/J 0 0 0 0 0 1 0 0 (ksf/mwf) (csf/mwf) (-ksf*a/mwf) (-csf*a/mwf) (-kwf-ksf)/mwf (-cwf-csf)/mwf 0 0 0 0 0 0 0 0 0 1 (ksr/mwr) (csr/mwr) (ksr*b/mwr) (csr*b/mwr) 0 0 (-kwr-ksr)/mwr (-cwr-csr)/mwr]; Bc=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 kwf/mwf csf/mwf 0 0 0 0 0 0 0 0 kwr/mwr csr/mwr]; Cc=[0 0 0 1 0 0 0 0]; Dc=zeros(1,4); %Criação do vetor de entradas da suspensão usus=zeros(4,length(t)); ws=2*pi*10/1.5; td=(a+b)/10; % lombada na origem for i=1:150 usus(1,i)=(0.08/2)*(1-cos(ws*i*dt)); usus(3,i+265)=(0.08/2)*(1-cos(ws*(i*dt))); end % %buraco longitudinal % for i=1:(0.08*1000) % usus(1,i)=-0.08; % usus(3,i+265)=-0.08; % end % %buraco transversal % for i=1:(0.08*1000) % usus(1,i)=-0.08; % end % %tachao % for i=1:(0.015*1000) % usus(1,i)=0.047; % end %Derivadas do vetor de entradas da suspensão aux1=diff(usus(1,:)/dt); usus(2,:)=[aux1(1) aux1]; aux2=diff(usus(3,:)/dt); usus(4,:)=[aux2(1) aux2]; %Simulação da suspensão sysc=ss(Ac,Bc,Cc,Dc); thetapsus = lsim(sysc,usus,t,X0c); thetapp = diff(thetapsus)/dt; thetappsus = [thetapp;thetapp(length(thetapp))]; %------------------------------------------------------- %---------------SIMULAÇÃO DA PLATAFORMA ---------------- %------------------------------------------------------- % Definição de parâmetros iniciais Jo = 0.0978/2; %kgm^2 Momento de inércia do Cardan Externo Ji = 0.0636/2; %kgm^2 Momento de inércia do Cardan Interno Jp = 9.3615/2; %kgm^2 Momento de inércia da Plataforma Co = 1.6; %Nms/rad Coeficiente de amortecimento do cardan externo Cp = 1.6; %Nms/rad Coeficiente de amortecimento do cardan interno Ci = 1.6; %Nms/rad Coeficiente de amortecimento da plataforma %Definição das entradas %Velocidades angulares do carro u=[p,pp,q,qp,r,rp] u=zeros(6,length(t)); % % Entrada senoidal % omega = 100; % u(1,:)=-1*cos(omega*ut)/omega; % u(2,:)=1*sin(omega*ut); %Entrada acoplada à suspensão u(3,:)=thetapsus'; u(4,:)=thetappsus'; % % Entrada impulso % u(1,:)=1; % u(2,1)=1000; u=transpose(u); %Vetor de estados: [theta, thetap, phi, phip, psi, psip] %Vetor de condições iniciais y0 = [pi/2 0 pi/2 0 pi/2 0]; %Matrizes do espaço de estados A = [0 1 0 0 0 0; 0 -Cp/Jp 0 Co/Jo 0 0; 0 0 0 1 0 0; 0 0 0 -Co/Jo 0 0; 0 0 0 0 0 1; 0 0 0 0 0 -Ci/(2*Ji)]; B = [0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 -1 0 0 0 0; 0 0 0 0 0 0; 0 0 0 -1 0 0]; C = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0]; D = zeros(size(C)); %Criação do sistema da plataforma sys = ss(A,B,C,D); %Simulação do sistema linear Ylin = lsim(sys,u,t,[0 0 0 0 0 0]); %Solução do sistema não linear [t,Ynl] = ode45(@(t,y) plataforma(t,y,ut,u),t,[pi/2 0 pi/2 0 pi/2 0]); XNL = [Ynl(:,1) Ynl(:,3) Ynl(:,5)]; %Criação dos vetores de saídas dtheta = Ylin(:,1); dphi = Ylin(:,2); dpsi = Ylin(:,3); theta = dtheta + y0(1); phi = dphi + y0(3); psi = dpsi + y0(5); %Matriz de transferência FT = tf(sys); %Simulação do sistema linear no domínio da frequência entrada_freq=4; thetas = lsim(FT(1,entrada_freq),u(:,entrada_freq),t,y0)+y0(1); phis = lsim(FT(2,entrada_freq),u(:,entrada_freq),t,y0)+y0(3); psis = lsim(FT(3,entrada_freq),u(:,entrada_freq),t,y0)+y0(5); %Polos e zeros ps = pole(sys); zs = zero(sys); %Integração por matriz de transição e termos forçantes toll=100; %precisão da expansão %Matriz de transição TM = zeros(size(A)); for i=1:toll aux = (A^(i-1))*(dt^(i-1))/factorial(i-1); TM = TM+aux; end %Matriz de termos forçantes FTM = zeros(size(A)); for i=1:toll aux = (A^(i-1))*(dt^(i-1))/factorial(i); FTM = FTM+aux; end FTM=dt*FTM; %Solução propriamente dita xMA(:,1)=y0;% condição inicial for j=1:length(t)-1 xMA(:,j+1) = TM*xMA(:,j)+FTM*B*transpose(u(j,:)); end XMT = transpose(C*xMA); %saídas por matriz de transição %XMT(:,1) = XMT(:,1) + y0(1); %XMT(:,2) = XMT(:,2) + y0(3); %XMT(:,3) = XMT(:,3) + y0(5); %Gráficos set(0,'defaulttextInterpreter','latex') % % %Gráfico dos ângulos por matriz de transição figure(1); hold on plot(t,XMT(:,1),'LineWidth',2); plot(t,XMT(:,2),'LineWidth',2); plot(t,XMT(:,3),'LineWidth',2); hold off hl = legend('\theta','\phi','\psi'); set(hl, 'interpreter', 'tex'); grid on; grid minor; title('Resposta do sistema linear utilizando solu\c{c}\~ao por Matriz de Transi\c{c}\~ao','Interpreter','latex'); xlabel('Tempo (s)','Interpreter','Latex'); ylabel('\^Angulo (rad)','Interpreter','Latex'); ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar(); % %Gráfico dos ângulos pelo comando lsim figure(2); hold on; plot(t,theta,'LineWidth',2); plot(t,phi,'LineWidth',2); plot(t,psi,'LineWidth',2); hold off; hl = legend('\theta','\phi','\psi'); set(hl, 'interpreter', 'tex'); title('Resposta do sistema linear'); xlabel('Tempo (s)','Interpreter','Latex'); ylabel('\^Angulo (rad)','Interpreter','Latex'); grid on; grid minor; ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar(); % % Gráfico de polos e zeros figure(3); p = pzplot(sysc); p.AxesGrid.XUnits = '$s^{-1}$'; p.AxesGrid.YUnits = '$s^{-1}$'; title('Polos do modelo da suspens\~ao de meio carro longitudinal','Interpreter','latex'); xlabel('Eixo real','Interpreter','latex'); ylabel('Eixo imagin\''ario','Interpreter','latex'); a = findobj(gca,'type','line'); for i = 1:length(a); set(a(i),'markersize',10); %change marker size set(a(i), 'linewidth',2);%change linewidth end ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar(); % %Gráfico da entrada figure(4); hold on; plot(t,u(:,3),'LineWidth',2); plot(t,u(:,4),'LineWidth',2); % plot(t,u(:,3),'LineWidth',2); % plot(t,u(:,4),'LineWidth',2); % plot(t,u(:,5),'LineWidth',2); % plot(t,u(:,6),'LineWidth',2); hold off; %hl = legend('$p (rad/s)$','$\dot{p} (rad/s^2)$','$q (rad/s)$','$\dot{q}(rad/s^2)$');%,'$r(rad/s)$','$\dot{r}(rad/s^2)$'); hl = legend('$q (rad/s)$','$\dot{q}(rad/s^2)$'); set(hl, 'interpreter', 'latex'); title('Entradas do sistema'); xlabel('Tempo (s)','interpreter','latex'); ylabel('Entrada','interpreter','latex'); grid on; grid minor; ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar(); % %Gráfico dos ângulos pelo comando ode45 figure(5); hold on; plot(t,XNL(:,1),'LineWidth',2); plot(t,XNL(:,2),'LineWidth',2); plot(t,XNL(:,3),'LineWidth',2); hold off; hl = legend('\theta','\phi','\psi'); set(hl, 'interpreter', 'tex'); title('Resposta do sistema n\~ao linear'); xlabel('Tempo (s)','Interpreter','Latex'); ylabel('\^Angulo (rad)','Interpreter','Latex'); trocar(); grid on; grid minor; ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar(); % %Gráficos de Bode figure(6); opts = bodeoptions('cstprefs'); opts.XLabel.String = 'Frequ\^encia'; opts.XLabel.Interpreter = 'latex'; opts.XLim={[1 10000]}; opts.Ylim={[-200 0],[-90 90]}; bodeplot(sys(1,2),opts); title(['Diagrama de Bode para ' '$\theta$' ' e ' '$\dot{p}$'],'Interpreter','latex'); axes=findobj('type','axes'); h_magnitude=get(axes(2),'YLabel'); h_phase=get(axes(1),'YLabel'); set(h_magnitude,'String','Magnitude (dB)','Interpreter','latex'); set(h_phase,'String','Fase ($$^{\circ}$$)','Interpreter','latex'); grid on; trocar(); % % figure(7); opts = bodeoptions('cstprefs'); opts.XLabel.String = 'Frequ\^encia'; opts.XLabel.Interpreter = 'latex'; opts.XLim={[1 10000]}; opts.Ylim={[-200 0],[-90 90]}; bodeplot(sys(2,2),opts); title(['Diagrama de Bode para ' '$\phi$' ' e ' '$\dot{p}$'],'Interpreter','latex'); axes=findobj('type','axes'); h_magnitude=get(axes(2),'YLabel'); h_phase=get(axes(1),'YLabel'); set(h_magnitude,'String','Magnitude (dB)','Interpreter','latex'); set(h_phase,'String','Fase ($$^{\circ}$$)','Interpreter','latex'); grid on; trocar(); % % figure(8); opts = bodeoptions('cstprefs'); opts.XLabel.String = 'Frequ\^encia'; opts.XLabel.Interpreter = 'latex'; opts.XLim={[1 10000]}; opts.Ylim={[-200 0],[-90 90]}; bodeplot(sys(3,4),opts); title(['Diagrama de Bode para ' '$\psi$' ' e ' '$\dot{q}$'],'Interpreter','latex'); axes=findobj('type','axes'); h_magnitude=get(axes(2),'YLabel'); h_phase=get(axes(1),'YLabel'); set(h_magnitude,'String','Magnitude (dB)','Interpreter','latex'); set(h_phase,'String','Fase ($$^{\circ}$$)','Interpreter','latex'); grid on; trocar(); % % Gráfico da entrada figure(9); hold on; aa = plot(t(1:500),usus(1,1:500),'LineWidth',2); bb = plot(t(1:500),usus(3,1:500),'LineWidth',2); hold off; hl = legend('$p (rad/s)$','$\dot{p} (rad/s^2)$','$q (rad/s)$','$\dot{q}(rad/s^2)$');%,'$r(rad/s)$','$\dot{r}(rad/s^2)$'); hl = legend('Via da roda 1','Via da roda 2'); set(hl, 'interpreter', 'latex'); title('Entradas do sistema'); xlabel('Tempo (s)','interpreter','latex'); ylabel('Entrada (m)','interpreter','latex'); grid on; grid minor; ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar(); %Gráfico dos ângulos pelo comando lsim no domínio da frequencia figure(10); hold on; plot(t,thetas(:,1),'LineWidth',2); plot(t,phis(:,1),'LineWidth',2); plot(t,psis(:,1),'LineWidth',2); hold off; hl = legend('\theta','\phi','\psi'); set(hl, 'interpreter', 'tex'); title('Resposta do sistema linear no dominio da frequencia','interpreter','latex'); xlabel('Tempo (s)','Interpreter','Latex'); ylabel('\^Angulo (rad)','Interpreter','Latex'); trocar(); grid on; grid minor; ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar(); % % Gráfico de polos e zeros figure(11); p = pzplot(sys); p.AxesGrid.XUnits = '$s^{-1}$'; p.AxesGrid.YUnits = '$s^{-1}$'; title('Polos do modelo da plataforma estabilizadora','Interpreter','latex'); xlabel('Eixo real','Interpreter','latex'); ylabel('Eixo imagin\''ario','Interpreter','latex'); a = findobj(gca,'type','line'); for i = 1:length(a); set(a(i),'markersize',10); %change marker size set(a(i), 'linewidth',2);%change linewidth end ax = gca; ax.XRuler.Exponent = 0; ax.YRuler.Exponent = 0; trocar();