（源代碼）PWPF調製

1、仿真來源：《應用時標分離和動態逆方法設計飛行器的姿態控制系統》，武立軍，賀有智，現代防禦技術，2007

clear
clc
[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;
        end
T=Tb+Lb*(H-Hb);
if Lb==0
    P=Pb*exp(-34.1631947*(H-Hb)/Tb);
else
    P=Pb*(Tb/T)^(34.1631947/Lb);
end
row=P/T/287.05;
av=20.0468*sqrt(T);
 
R=6371004;
g=(6371004/(6371004+YH))^2*9.80665;