六自由度导弹simulink仿真建模

本人为研一学生，第一学期导师安排学习六自由度基础仿真建模，搞清楚整体流程，方便后续制导控制工作展开，代码均是师兄师姐源码修改而来，笔者才疏学浅，如有错误希望读者可以指出，共同学习。

本次仿真为导弹六自由度建模，包含两种控制方法，过载控制和姿态控制。

一、程序内容

1.1 初始文件sim_init.m

初始化参数及运动状态。

%初始化参数
S_ref = 0.110447; %参考面积
L_ref = 6.2; %参考长度
L = 6.2;
S = 0.110447;
HD = 57.3;

%初始运动状态
mass_init = Mass_dandao(240);

Vmx_init = Vw_dandao(240); %初始速度   初始化
Vmy_init = 0;
Vmz_init = 0;

init_xm0 = 0; %初始位置
init_ym0 = 2000; 
init_zm0 = 0; 

wx0 = wx_dandao(240)/HD;%初始角速度-0.0132536329000000
wy0 = wy_dandao(240)/HD;%9.50546595000000e-05
wz0 = wz_dandao(240)/HD;%-6.40883682000000e-05

theta_0 = Theta_dandao(240)/HD;%初始欧拉角
psi_0 = psi_dandao(240)/HD;
gama_0 = phi_dandao(240)/HD;


g = 9.80665;%重力系数
L_0 = 104.21631;%发射点经纬高  初始化
B_0 = 41.18279;
H0 = 10000;

L_0t = 104.21631;%经纬高
B_0t = 41.18279;
H0t = 100;

L_t = 98.49998;%目标经纬高
B_t = 38.99999;
H_t = 198.64;

1.2 动力学系数kinetics_coefficient.m

包含俯仰偏航滚转三个通道的动力学系数计算，对纵向和横向导数小扰动线性化部分。

其中升力系数和阻力系数使用插值表计算得到。

例如计算俯仰通道动力学系数：

%%  %%%%%%%俯仰通道动力学系数计算
  for aa = 1:length(alphac_inter) 
      for ma = 1:length(Ma3) 
         for dz = 1:length(deltaz_inter)
             for hh = 1:length(h) 
                for m = 1:length(m3)                
              a1=0;
              a2(aa,ma,m,hh) = -57.3*Cmz_alpha(aa,ma,m)*q(ma,hh)*S*L/Iz3(m); %量级10^2
              a3(aa,ma,dz,m,hh) = -57.3*Cmz_deltaz(aa,dz,ma,m)*q(ma,hh)*S*L/Iz3(m);   %量级10^2       
              a4(aa,ma,m,hh) =(57.3*Y_alpha(aa,ma)*q(ma,hh)*S )/m3(m)/v(ma,hh);  %量级 0.1  缺推力P
              a5(aa,ma,dz,m,hh) = 57.3*Y_deltaz(aa,dz,ma)*q(ma,hh)*S/m3(m)/v(ma,hh);  %量级0.01
              KL_alpha(aa,ma,m,hh) = 57.3*Y_alpha(aa,ma)*q(ma,hh)*S_ref/m3(m)/g;
              KL_delta(aa,ma,dz,m,hh) = 57.3*Y_deltaz(aa,dz,ma)*q(ma,hh)*S_ref/m3(m)/g;

%             KL_alpha_phase1(mm,hh,aa,bb) = 57.3*Cy_alpha_config1(aa,bb,mm)*q_phase1(mm,hh)*S_ref_phase1_2/mass_vector_phase1(mm)/g;
%             KL_delta_phase1(mm,hh,aa,bb) = 57.3*Cy_deltaz_config1(aa,bb,mm)*q_phase1(mm,hh)*S_ref_phase1_2/mass_vector_phase1(mm)/g;
           
                end
             end
          end
      end
 end

1.3 过载控制模块n_controller.m

先计算飞行包线，动压分区，再取定攻角侧滑角舵偏角的设计点，设计三个通道的过载回路

%% %%%%%%动压分区
for hh=1:length(h)
    for mm=1:length(Ma3)
        if  q(mm,hh)<5.0e+004 
            q_zone(mm,hh)=1;
        elseif q(mm,hh)<8.0e+004 && q(mm,hh)>=5.0e+004
            q_zone(mm,hh)=2;
        elseif q(mm,hh)<1.1e+005 &&q(mm,hh)>=8.0e+004
            q_zone(mm,hh)=3;
        elseif q(mm,hh)<1.5e+005 && q(mm,hh)>=1.1e+005
            q_zone(mm,hh)=4;
        elseif q(mm,hh)<2e+005 && q(mm,hh)>=1.5e+005
            q_zone(mm,hh)=5;
        elseif q(mm,hh)<2.8e+005 && q(mm,hh)>=2e+005
            q_zone(mm,hh)=6;
        elseif q(mm,hh)<4e+005 && q(mm,hh)>=2.8e+005
            q_zone(mm,hh)=7;
        elseif q(mm,hh)<5.6e+005 && q(mm,hh)>=4e+005
            q_zone(mm,hh)=8;
        elseif q(mm,hh)<7e+005 && q(mm,hh)>=5.6e+005
            q_zone(mm,hh)=9;
        end
    end
end

%% 取攻角2°，侧滑角0°，舵偏5°为设计点
for aa = 1:length(alphac_inter)%[0	2	4	6	8	10	15	20]
        for mm = 1:length(Ma3)%[2  2.5  3]
            for dz = 1:length(deltaz_inter)%[-15	-10	-5	0	5	10	15]
                for hh = 1:length(h)%[ 10	  2000	 4000	6000	8000	10000	12000 ]
                    a1_design_n = 0;
                    a2_design_n(mm,hh) = a2(2,mm,2,hh);  % alpha(2)  ma  m3(1019.67)  h
                    a3_design_n(mm,hh) = a3(2,mm,3,2,hh);% alpha(2)  ma   deltaz(-5)   m3(1019.67)  h
                    a4_design_n(mm,hh) = a4(2,mm,2,hh);
                    a5_design_n(mm,hh) = a5(2,mm,3,2,hh);
                    KL_alpha_design_n(mm,hh) = KL_alpha(2,mm,2,hh);
                            
                    a1_autopilot_n=0;
                    a2_autopilot_n(aa,mm,hh) = a2(aa,mm,2,hh);
                    a3_autopilot_n(aa,mm,dz,hh) = a3(aa,mm,dz,2,hh);
                    a4_autopilot_n(aa,mm,hh) = a4(aa,mm,2,hh);
                    a5_autopilot_n(aa,mm,dz,hh) = a5(aa,mm,dz,2,hh);                 
                    KL_alpha_autopilot_n(aa,mm,hh) = KL_alpha(aa,mm,2,hh);  
                    KL_delta_autopilot_n(aa,mm,hh) = KL_delta(aa,mm,2,hh);
                end
            end
        end
end

%% 按动压分区，一个高度一个指标
kesi_phase3 = [0.8  0.8  0.8  0.8  0.9  0.9  0.9  1  1]*1.3;
wd_phase3 = [10 10 10 10 10 10 10 10 10]*1.3;
Tr_phase3 =[1  1  1  0.6  0.6 0.6 0.5 0.5 0.5];
%% 过载回路设计
for mm = 1:length(Ma3)%[2  2.5  3]
    for hh = 1:length(h)%[ 10	  2000	 4000	6000	8000	10000	12000 ]
        k_omiga_pitch_phase3(mm,hh) = -2*kesi_phase3(q_zone(mm,hh))*wd_phase3(q_zone(mm,hh))/a3_design_n(mm,hh);%%%
        k_alpha_pitch_phase3(mm,hh) = (wd_phase3(q_zone(mm,hh))^2 - a2_design_n(mm,hh))...
            /(2*kesi_phase3(q_zone(mm,hh))*wd_phase3(q_zone(mm,hh)));%%%
        %             k_n_pitch(mm,hh,tt) = KK(q_zone(mm,hh))/KL_alpha_design(mm,hh,tt);%%%
        k_n_pitch_phase3(mm,hh) = (9.81*2.2*wd_phase3(q_zone(mm,hh))^2)/((wd_phase3(q_zone(mm,hh))^2-...
            a2_design_n(mm,hh))*a4_design_n(mm,hh)*c_n(hh)*Ma3(mm)*Tr_phase3(q_zone(mm,hh)));
    end
end

1.4 姿态控制模块attitude_control.m

三个通道需要分别设计内环穿越频率，姿态回路增益以及姿态回路积分增益

%% %%%%%偏航通道控制器 取攻角0°，侧滑角2°，舵偏-5°为设计点

for bb = 1:length(alphac_inter)%[0	 2	 4	6	8	10	15	20]   1*8
     for mm = 1:length(Ma3)%[2	 2.5  3]  1*3
            for dy = 1:length(deltay_inter)%[-15 -10  -5  0	 5	10	15]  1*7
                for hh = 1:length(h)%[10	2000	4000	6000	8000	10000	12000]  1*7
                  
                    b1_design = 0;
                    b2_design(mm,hh) = b2(2,mm,2,hh);
                    b3_design(mm,hh) = b3(2,mm,3,2,hh);
                    b4_design(mm,hh) = b4(2,mm,2,hh);
                    b5_design(mm,hh) = b5(2,mm,3,2,hh);
%                     KL_alpha_design_phase1(mm,hh) = KL_alpha_phase1(mm,hh,4,2);
                    
                    b1_autopilot=0;
                    b2_autopilot(bb,mm,hh) = b2(bb,mm,2,hh);
                    b3_autopilot(bb,mm,dy,hh) = b3(bb,mm,dy,2,hh);
                    b4_autopilot(bb,mm,hh) = b4(bb,mm,2,hh);
                    b5_autopilot(bb,mm,dy,hh) = b5(bb,mm,dy,2,hh);
%                     KL_alpha_autopilot(mm,hh,aa,bb) = KL_alpha_phase1(mm,hh,aa,bb);
%                     KL_delta_autopilot(mm,hh,aa,bb) = KL_delta_phase1(mm,hh,aa,bb);
              
                end
            end
     end
end


%% %%按动压分区，一个高度一个指标
omiga_c2 = 1*[10 10 10 10 10 13 22 24 24];%内环穿越频率
k_psi_temp = 2*[2 2 2 2 2 2 3 3.5 3.5 ];%姿态回路增益
k_psiI_temp =10*[-2 -2 -2 -2 -2 -2 -2 -2 -2];%姿态回路积分增益
for mm = 1:length(Ma3) 
    for hh=1:length(h)
        %         omiga_c(mm,hh) = 2*kesi_temp(mm,hh)*omiga_d_temp(mm,hh);
        k_r_yaw(mm,hh) = -abs(omiga_c2(q_zone(mm,hh))/b3_design(mm,hh));
        k_psi_yaw(mm,hh) = k_r_yaw(mm,hh)*k_psi_temp(q_zone(mm,hh));
        k_psiI_yaw(mm,hh) = 0.1*k_psiI_temp(q_zone(mm,hh));
    end
end

1.5 画图模块plot_dandao.m

close all
hold on
H=simout(:,1);
ktheta_out=simout(:,3);
psi_out=simout(:,4);
gama_out=simout(:,5);

x_out=simout(:,6);
y_out=simout(:,7);
z_out=simout(:,8);

vx_out=simout(:,9);
vy_out=simout(:,10);
vz_out=simout(:,11);

alpha_out=simout(:,12);
beta_out=simout(:,13);

kthetac_out=simout(:,20);
psic_out=simout(:,21);
gamac_out=simout(:,22);


plot(tout,H,'linewidth',2);
xlabel('时间（s）');
ylabel('高度（m）');
grid on
title('高度随时间变化曲线');


figure
plot(tout,alpha_out,'linewidth',2);
hold on
plot(tout,beta_out,'r--','linewidth',2);
xlabel('时间（s）');
ylabel('攻角侧滑角（\circ）');
legend('攻角曲线','侧滑角曲线')
title('攻角侧滑角随时间变化曲线');
grid on

figure
plot3(x_out, z_out, y_out, 'b-', 'LineWidth', 2);
xlabel('Z轴');
ylabel('X轴');
zlabel('Y轴');
title('三维飞行轨迹');
grid on;

二、SIXD.slx simulink整体仿真

整体

控制律模块

动力学模块

仿真结果主要查看控制模块是否可以正常工作，如果用过载控制，假如给定法向和切向过载指令为2，系统能够稳定跟上指令；如果用姿态控制，给定合适范围内的姿态角指令，控制器模块也能很迅速很稳定地跟上指令。