（源代码）PWPF调制

来源：互联网发布：办公软件合集下载编辑：程序博客网时间：2024/05/21 06:12

1、仿真来源：《应用时标分离和动态逆方法设计飞行器的姿态控制系统》，武立军，贺有智，现代防御技术，2007

clearclc[row,av,g]=OnAir(50000);  %计算大气密度，当地声速，重力加速度m=44; %质量 Tay=60*g;  %姿控发动机推力L=1.945;   %抛盖后长度DD=0.37;%弹体直径Jx=0.3;   %绕x轴转动惯量Jy=3.9;  %绕y轴转动惯量Jz=3.9;  %绕z轴转动惯量xcg=0.47*L;   %质心位置，单位m vartheta=0.1;fea=0.1;gamma=0.1;omega_x=0;omega_y=0;omega_z=0; %控制器参数sum1=0;sum2=0;sum3=0;k1=5;k2=5;k3=5;m=40;n=400; %PWPF调制器参数%滚转通道Km1=1;Tm1=1;U1=0;Uon1=0.1;Uoff1=0.01;u_out1=0;  %取值为0 1 -1%偏航通道Km2=1;Tm2=1;U2=0;Uon2=0.8;Uoff2=0.3;u_out2=0;  %取值为0 1 -1%俯仰通道Km3=1;Tm3=0.85;U3=0;Uon3=1.4;Uoff3=1.35;u_out3=0;  %取值为0 1 -1  Dt=0.001;n=1;t=0;for i=1:5000    %期望姿态角    vartheta_c=50*pi/180;    fea_c=40*pi/180;    gamma_c=30*pi/180;          e1=[vartheta-vartheta_c;        fea-fea_c;        gamma-gamma_c];    k_matrix=[-k1 0 0;        0 -k2 0;        0 0 -k3];    tran1=[0 sin(gamma) cos(gamma);        0 cos(gamma)/cos(vartheta) -sin(gamma)/cos(vartheta);        1 -tan(vartheta)*cos(gamma) tan(vartheta)*sin(gamma)];    omega_c=inv(tran1)*k_matrix*e1;        e2=[omega_x-omega_c(1);        omega_y-omega_c(2);        omega_z-omega_c(3);];    tran2=[1/Jx 0 0;        0 1/Jy 0;        0 0 1/Jz];    sum1=sum1+e2(1)*Dt;    sum2=sum2+e2(2)*Dt;    sum3=sum3+e2(3)*Dt;    m_matrix=[-m*e2(1)-n*sum1-(Jy-Jz)/Jx*omega_y*omega_z;        -m*e2(2)-n*sum2-(Jz-Jx)/Jy*omega_x*omega_z;        -m*e2(3)-n*sum3-(Jx-Jy)/Jz*omega_x*omega_y];    u=inv(tran2)*m_matrix;        %对滚转通道力矩调制    Du1=-U1/Tm1+Km1/Tm1*(u(1)-u_out1);    U1=U1+Du1*Dt;    if u_out1==-1*Tay*2*DD/2        if U1>-Uoff1            u_out1=0;        else            u_out1=-1*Tay*2*DD/2;        end    elseif u_out1==0        if U1>Uon1            u_out1=1*Tay*2*DD/2;        elseif U1>-Uon1            u_out1=0;        else            u_out1=-1*Tay*2*DD/2;        end    else%if Aout==1         if U1<Uoff1            u_out1=0;        else            u_out1=1*Tay*2*DD/2;        end    end     %对偏航通道力矩调制    Du2=-U2/Tm2+Km2/Tm2*(u(2)-u_out2);    U2=U2+Du2*Dt;    if u_out2==-1*Tay*2*(L-xcg)        if U2>-Uoff2            u_out2=0;        else            u_out2=-1*Tay*2*(L-xcg);        end    elseif u_out2==0        if U2>Uon2            u_out2=1*Tay*2*(L-xcg);        elseif U2>-Uon2            u_out2=0;        else            u_out2=-1*Tay*2*(L-xcg);        end    else%if Aout==1         if U2<Uoff2            u_out2=0;        else            u_out2=1*Tay*2*(L-xcg);        end    end     %对俯仰通道力矩调制    Du3=-U3/Tm3+Km3/Tm3*(u(3)-u_out3);    U3=U3+Du3*Dt;    if u_out3==-1*Tay*2*(L-xcg)        if U3>-Uoff3            u_out3=0;        else            u_out3=-1*Tay*2*(L-xcg);        end    elseif u_out3==0        if U3>Uon3            u_out3=1*Tay*2*(L-xcg);        elseif U3>-Uon3            u_out3=0;        else            u_out3=-1*Tay*2*(L-xcg);        end    else%if Aout==1         if U3<Uoff3            u_out3=0;        else            u_out3=1*Tay*2*(L-xcg);        end    end        Domega_x=(u_out1-(Jz-Jy)*omega_y*omega_z)/Jx;    Domega_y=(u_out2-(Jx-Jz)*omega_x*omega_z)/Jy;    Domega_z=(u_out3-(Jy-Jx)*omega_y*omega_x)/Jz;    omega_x=omega_x+Domega_x*Dt;    omega_y=omega_y+Domega_y*Dt;    omega_z=omega_z+Domega_z*Dt;    D=tran1*[omega_x;omega_y;omega_z];    vartheta=vartheta+D(1)*Dt;    fea=fea+D(2)*Dt;    gamma=gamma+D(3)*Dt;     vartheta_store(:,n)=[vartheta_c;vartheta];    fea_store(:,n)=[fea_c;fea];    gamma_store(:,n)=[gamma_c;gamma];    u_store(:,n)=[u(1);u(2);u(3);u_out1;u_out2;u_out3;];    w_store(:,n)=[omega_c(1);omega_c(2);omega_c(3);omega_x;omega_y;omega_z];     n=n+1;    t=t+Dt;end% figure(1)% plot((1:n-1)*Dt,u_store(1,:),(1:n-1)*Dt,u_store(4,:))% xlabel('time/s')% ylabel('M_x/N*m')% figure(2)% plot((1:n-1)*Dt,u_store(2,:),(1:n-1)*Dt,u_store(5,:))% xlabel('time/s')% ylabel('M_y/N*m')% figure(3)% plot((1:n-1)*Dt,u_store(3,:),(1:n-1)*Dt,u_store(6,:))% xlabel('time/s')% ylabel('M_z/N*m') figure(4)plot((1:n-1)*Dt,gamma_store(1,:)*180/pi,(1:200:n-1)*Dt,gamma_store(2,1:200:end)*180/pi,'r+')legend('指令滚转角','实际滚转角')xlabel('时间/s')ylabel('\gamma/°') figure(5)plot((1:n-1)*Dt,vartheta_store(1,:)*180/pi,(1:200:n-1)*Dt,vartheta_store(2,1:200:end)*180/pi,'r+')legend('指令俯仰角','实际俯仰角')xlabel('时间/s')ylabel('\vartheta/°') figure(6)plot((1:n-1)*Dt,fea_store(1,:)*180/pi,(1:200:n-1)*Dt,fea_store(2,1:200:end)*180/pi,'r+')legend('指令偏航角','实际偏航角')xlabel('时间/s')ylabel('\psi/°') figure(7)plot((1:n-1)*Dt,w_store(4,:),(1:n-1)*Dt,w_store(5,:),(1:n-1)*Dt,w_store(6,:))legend('\omega_x','\omega_y','\omega_z')title('弹体旋转角速度')xlabel('时间/s')ylabel('角速度/rad/s') figure(8)plot((1:n-1)*Dt,w_store(1,:),(1:200:n-1)*Dt,w_store(4,1:200:end),'r+')legend('\omega_x_c','\omega_x')title('弹体旋转角速度')xlabel('时间/s')ylabel('角速度/rad/s') figure(9)plot((1:n-1)*Dt,w_store(2,:),(1:200:n-1)*Dt,w_store(5,1:200:end),'r+')legend('\omega_y_c','\omega_y')title('弹体旋转角速度')xlabel('时间/s')ylabel('角速度/rad/s') figure(10)plot((1:n-1)*Dt,w_store(3,:),(1:200:n-1)*Dt,w_store(6,1:200:end),'r+')legend('\omega_z_c','\omega_z')title('弹体旋转角速度')xlabel('时间/s')ylabel('角速度/rad/s') figure(11)subplot(3,1,1)plot((1:n-1)*Dt,u_store(4,:))xlabel('time/s')ylabel('M_x/N*m')%figure(12)subplot(3,1,2)plot((1:n-1)*Dt,u_store(5,:))xlabel('time/s')ylabel('M_y/N*m')%figure(13)subplot(3,1,3)plot((1:n-1)*Dt,u_store(6,:))xlabel('time/s')ylabel('M_z/N*m')

OnAir.m子程序

function [row,av,g]=OnAir(YH) H=6356.766*YH/(6356766+YH);if H<0    H=0;elseif H<=11        Tb=288.15;        Lb=-6.5;        Pb=101325.3;        Hb=0;    elseif H<=20        Tb=216.65;        Lb=0;        Pb=22631.99;        Hb=11.0;    elseif H<=32        Tb=216.65;        Lb=1;        Pb=5474.798;        Hb=20.0;    elseif H<=47        Tb=228.65;        Lb=2.8;        Pb=868.0101;        Hb=32;    elseif H<=51        Tb=270.65;        Lb=0;        Pb=110.9005;        Hb=47;    elseif H<=71        Tb=270.65;        Lb=-2.8;        Pb=66.93803;        Hb=51;    elseif H<=84            Tb=214.65;            Lb=-2.0;            Pb=4.220234;            Hb=71;        else            row =0;            av=340;        endT=Tb+Lb*(H-Hb);if Lb==0    P=Pb*exp(-34.1631947*(H-Hb)/Tb);else    P=Pb*(Tb/T)^(34.1631947/Lb);endrow=P/T/287.05;av=20.0468*sqrt(T); R=6371004;g=(6371004/(6371004+YH))^2*9.80665;

0 0