弹箭六自由度弹道计算程序(c++，vs 2017)

以前写弹道程序都是使用 matlab，前两天心血来潮，突然想编写一个c++编写的六自由度弹道程序，所用时间大概3天左右，好了，不废话了，开始进入正题…

1、参考文献

使用的参考书是韩子鹏编写的《弹箭外弹道学》。本文源代码里面的参数命名参考《弹箭外弹道学》第六章，与其一致。六自由度弹箭弹道微分方程组如下：

总共有15个变量和15个方程，当已知弹箭结构参数、气动力参数射击条件、气象条件、起始条件，就积分得到弹箭的运动规律和任一时刻的弹道诸元。

2、程序内容

弹道学知识在这里不展开叙述，读者可通过《弹箭外弹道学》进行学习了解。在这介绍六自由度弹箭弹道计算程序的框架。

程序框架分为5个部分：参数输入、弹道微分方程组函数、龙格库塔函数、其它相关函数、结果输出函数

（1）参数输入部分

参数输入部分包括：大气环境参数、弹箭结构参数、弹箭气动参数。

1 、大气环境参数

private:const double G0 = 9.7803253359;        // 地球表面重力加速度(m/s2)const double rho0 = 1.205;             // 地面大气密度（kg/m3）const double T0 = 288.15;              // 地面温度（K）const double tatal0 = 289.1;           // 地面虚温（K）const double P0 = 0.1e6;               // 地面气压（MPa）const double R1 = 287;                 // 空气常数 J/KG/Kconst double k_gas = 1.4;              // 比热比

2、弹箭结构参数

随便找了一个155榴弹的结构参数

    double Mass = 42.76;                    // 弹丸重量（kg）double D = 0.155;                       // 弹丸口径（m）double S = PI * D*D / 4;                // 弹丸横截面积（m2）double L = 0.786;                       // 参考长度double Alph_W = DegToRad(30);           // 风的来向与正北方向的夹角（deg）double Alph_N = DegToRad(10);           // 射击方向与正北方向的夹角（deg）double k = 17;                          // 诱导阻力系数double A = 1.793;                       // 赤道转动惯量 kg*m2double C = 0.142;                       // 极转动惯量

3、弹箭气动参数
使用 MissileDatcome（如无此软件可进入作者主页进行下载软件安装包和软件使用手册）对155mm榴弹进行气动计算，程序中定义马赫数、零升阻力系数、升力系数导数、俯仰力矩系数导数、俯仰力矩系数导数、俯仰阻尼力矩系数、极阻尼力矩系数、马格努斯力矩系数、马格努斯力系数的 vector 数组用来存放读取的气动参数。

/*********************用来存放气动参数******************************/
std::vector<double> MACHs;         // 马赫数
std::vector<double> CD0s;          // 零升阻力系数
std::vector<double> CL_alphs;      // 升力系数导数（关于攻角）
std::vector<double> mz_alphs;      // 俯仰力矩系数导数（关于攻角）
std::vector<double> mzz_alphs;     // 俯仰阻尼力矩系数（关于攻角）
std::vector<double> mxz_alphs;     // 极阻尼力矩系数（关于滚转速率）
std::vector<double> my_alphs;      // 马格努斯力矩系数（关于攻角）
std::vector<double> cz_alphs;      // 马格努斯力系数（关于攻角）
/*****************************************************************/

（2）弹道微分方程组函数部分

为了避免文章篇幅过长，这里只展示了弹道微分方程组的函数声明，如下：

/*弹道微分方程组函数声明*/double func_v(double m, double F_x2);double func_theat_a(double m, double v, double psi_2, double F_y2);double func_psi_2(double m, double v, double F_z2);double func_omega_xi(double C, double M_xi);double func_omega_Eta(double A, double M_Eta, double C, double omega_xi, double omega_zeta, double phi_2);double func_omega_zeta(double A, double M_zeta, double C, double omega_xi, double omega_Eta, double omega_zeta, double phi_2);double func_phi_a(double omega_zeta, double phi_2);double func_phi_2(double omega_Eta);double func_gamma(double omega_xi, double omega_zeta, double phi_2);double func_x(double v, double psi_2, double theat_a);double func_y(double v, double psi_2, double theat_a);double func_z(double v, double psi_2);double sin_delta_2(double psi_2, double phi_2, double phi_a, double theta_a);double sin_delta_1(double phi_2, double phi_a, double theta_a, double delta_2);double beta(double psi_2, double phi_a, double theta_a, double delta_2);double F_X2(double P, double y, double vr, double v, double delta_r, double wx2, double delta_1, double delta_2,double vr_xi, double wz2, double wy2, double theat_a, double psi_2);double F_Y2(double P, double y, double vr, double v, double delta_r, double wy2, double delta_1, double delta_2,double vr_zeta, double wx2, double wz2, double theat_a);double F_Z2(double P, double y, double vr, double v, double delta_r, double wy2, double delta_1, double delta_2,double vr_zeta, double wx2, double wz2, double theat_a, double psi_2);double M_xi(double P, double y, double vr, double omega_xi, double v);double M_eta(double P, double y, double vr, double delta_r, double vr_zeta, double v, double omega_Eta,double omega_xi, double vr_eta);double M_zeta(double P, double y, double vr, double delta_r, double v, double vr_eta, double omega_zeta,double omega_xi, double vr_zeta);double vr(double v, double wx2, double wy2, double wz2);double delta_r(double vr_xi, double vr);double vr_xi(double v, double wx2, double delta_2, double delta_1, double wy2, double wz2);double vr_eta(double vr_eta2, double beta, double vr_zeta2);double vr_zeta(double vr_eta2, double beta, double vr_zeta2);double vr_eta2(double v, double wx2, double delta_1, double wy2);double vr_zeta2(double v, double wx2, double delta_2, double delta_1, double wy2, double wz2);double wx2(double wx, double psi_2, double theat_a, double wz);double wy2(double wx, double theat_a);double wz2(double wx, double psi_2, double theat_a, double wz);double wx(double w, double Alph_W, double Alph_N);double wz(double w, double Alph_W, double Alph_N);

（3）龙格库塔函数部分

这里弹道解法采用的是4阶龙格库塔法，对于大多数实际问题，4阶的龙格库塔法已经可以满足精度要求。时间步长的选择必须小于0.005s，否则计算发散。

/***********************************龙格库塔求解函数****************************/
void Trajectory_6FreedomDegree::RK4(double & v, double & theat_a, double & psi_2, double & omega_xi, double & omega_Eta, double & omega_zeta, double & phi_a, double & phi_2, double & gamma, double & x, double & y, double & z, double hdt, double m, double F_x2, double F_y2, double F_z2, double C, double M_xi, double A, double M_Eta, double M_zeta)
{Trajectory_6FreedomDegree RK_1;    // 创建一个对象，用来调用类 Trajectory_6FreedomDegree 里面的函数完成 龙格-库塔的计算。double K1_v, K2_v, K3_v, K4_v;double K1_theat_a, K2_theat_a, K3_theat_a, K4_theat_a;double K1_psi_2, K2_psi_2, K3_psi_2, K4_psi_2;double K1_omega_xi, K2_omega_xi, K3_omega_xi, K4_omega_xi;double K1_omega_Eta, K2_omega_Eta, K3_omega_Eta, K4_omega_Eta;double K1_omega_zeta, K2_omega_zeta, K3_omega_zeta, K4_omega_zeta;double K1_phi_a, K2_phi_a, K3_phi_a, K4_phi_a;double K1_phi_2, K2_phi_2, K3_phi_2, K4_phi_2;double K1_gamma, K2_gamma, K3_gamma, K4_gamma;double K1_x, K2_x, K3_x, K4_x;double K1_y, K2_y, K3_y, K4_y;double K1_z, K2_z, K3_z, K4_z;double v_1, theat_a_1, psi_2_1, omega_xi_1, omega_Eta_1, omega_zeta_1, phi_a_1, phi_2_1,gamma_1, x_1, y_1, z_1;K1_v = RK_1.func_v(m, F_x2);K1_theat_a = RK_1.func_theat_a(m, v, psi_2, F_y2);K1_psi_2 = RK_1.func_psi_2(m, v, F_z2);K1_omega_xi = RK_1.func_omega_xi(C, M_xi);K1_omega_Eta = RK_1.func_omega_Eta(A, M_Eta, C, omega_xi, omega_zeta, phi_2);K1_omega_zeta = RK_1.func_omega_zeta(A, M_zeta, C, omega_xi, omega_Eta, omega_zeta, phi_2);K1_phi_a = RK_1.func_phi_a(omega_zeta, phi_2);K1_phi_2 = RK_1.func_phi_2(omega_Eta);K1_gamma = RK_1.func_gamma(omega_xi, omega_zeta, phi_2);K1_x = RK_1.func_x(v, psi_2, theat_a);K1_y = RK_1.func_y(v, psi_2, theat_a);K1_z = RK_1.func_z(v, psi_2);v_1 = v + hdt * K1_v;theat_a_1 = theat_a + hdt * K1_theat_a;psi_2_1 = psi_2 + hdt * K1_psi_2;omega_xi_1 = omega_xi + hdt * K1_omega_xi;omega_Eta_1 = omega_Eta + hdt * K1_omega_Eta;omega_zeta_1 = omega_zeta + hdt * K1_omega_zeta;phi_a_1 = phi_a + hdt * K1_phi_a;phi_2_1 = phi_2 + hdt * K1_phi_2;gamma_1 = gamma + hdt * K1_gamma;x_1 = x + hdt * K1_x;y_1 = y + hdt * K1_y;z_1 = z + hdt * K1_z;K2_v = RK_1.func_v(m, F_x2);K2_theat_a = RK_1.func_theat_a(m, v_1, psi_2_1, F_y2);K2_psi_2 = RK_1.func_psi_2(m, v_1, F_z2);K2_omega_xi = RK_1.func_omega_xi(C, M_xi);K2_omega_Eta = RK_1.func_omega_Eta(A, M_Eta, C, omega_xi_1, omega_zeta_1, phi_2_1);K2_omega_zeta = RK_1.func_omega_zeta(A, M_zeta, C, omega_xi_1, omega_Eta_1, omega_zeta_1, phi_2_1);K2_phi_a = RK_1.func_phi_a(omega_zeta_1, phi_2_1);K2_phi_2 = RK_1.func_phi_2(omega_Eta_1);K2_gamma = RK_1.func_gamma(omega_xi_1, omega_zeta_1, phi_2_1);K2_x = RK_1.func_x(v_1, psi_2_1, theat_a_1);K2_y = RK_1.func_y(v_1, psi_2_1, theat_a_1);K2_z = RK_1.func_z(v_1, psi_2_1);v_1 = v + hdt * K2_v;theat_a_1 = theat_a + hdt * K2_theat_a;psi_2_1 = psi_2 + hdt * K2_psi_2;omega_xi_1 = omega_xi + hdt * K2_omega_xi;omega_Eta_1 = omega_Eta + hdt * K2_omega_Eta;omega_zeta_1 = omega_zeta + hdt * K2_omega_zeta;phi_a_1 = phi_a + hdt * K2_phi_a;phi_2_1 = phi_2 + hdt * K2_phi_2;gamma_1 = gamma + hdt * K2_gamma;x_1 = x + hdt * K2_x;y_1 = y + hdt * K2_y;z_1 = z + hdt * K2_z;K3_v = RK_1.func_v(m, F_x2);K3_theat_a = RK_1.func_theat_a(m, v_1, psi_2_1, F_y2);K3_psi_2 = RK_1.func_psi_2(m, v_1, F_z2);K3_omega_xi = RK_1.func_omega_xi(C, M_xi);K3_omega_Eta = RK_1.func_omega_Eta(A, M_Eta, C, omega_xi_1, omega_zeta_1, phi_2_1);K3_omega_zeta = RK_1.func_omega_zeta(A, M_zeta, C, omega_xi_1, omega_Eta_1, omega_zeta_1, phi_2_1);K3_phi_a = RK_1.func_phi_a(omega_zeta_1, phi_2_1);K3_phi_2 = RK_1.func_phi_2(omega_Eta_1);K3_gamma = RK_1.func_gamma(omega_xi_1, omega_zeta_1, phi_2_1);K3_x = RK_1.func_x(v_1, psi_2_1, theat_a_1);K3_y = RK_1.func_y(v_1, psi_2_1, theat_a_1);K3_z = RK_1.func_z(v_1, psi_2_1);v_1 = v + hdt * K3_v;theat_a_1 = theat_a + hdt * K3_theat_a;psi_2_1 = psi_2 + hdt * K3_psi_2;omega_xi_1 = omega_xi + hdt * K3_omega_xi;omega_Eta_1 = omega_Eta + hdt * K3_omega_Eta;omega_zeta_1 = omega_zeta + hdt * K3_omega_zeta;phi_a_1 = phi_a + hdt * K3_phi_a;phi_2_1 = phi_2 + hdt * K3_phi_2;gamma_1 = gamma + hdt * K3_gamma;x_1 = x + hdt * K3_x;y_1 = y + hdt * K3_y;z_1 = z + hdt * K3_z;K4_v = RK_1.func_v(m, F_x2);K4_theat_a = RK_1.func_theat_a(m, v_1, psi_2_1, F_y2);K4_psi_2 = RK_1.func_psi_2(m, v_1, F_z2);K4_omega_xi = RK_1.func_omega_xi(C, M_xi);K4_omega_Eta = RK_1.func_omega_Eta(A, M_Eta, C, omega_xi_1, omega_zeta_1, phi_2_1);K4_omega_zeta = RK_1.func_omega_zeta(A, M_zeta, C, omega_xi_1, omega_Eta_1, omega_zeta_1, phi_2_1);K4_phi_a = RK_1.func_phi_a(omega_zeta_1, phi_2_1);K4_phi_2 = RK_1.func_phi_2(omega_Eta_1);K4_gamma = RK_1.func_gamma(omega_xi_1, omega_zeta_1, phi_2_1);K4_x = RK_1.func_x(v_1, psi_2_1, theat_a_1);K4_y = RK_1.func_y(v_1, psi_2_1, theat_a_1);K4_z = RK_1.func_z(v_1, psi_2_1);v = v + hdt * (K1_v + 2.0*K2_v + 2.0*K3_v + K4_v) / 6.0;theat_a = theat_a + hdt * (K1_theat_a + 2.0*K2_theat_a + 2.0*K3_theat_a + K4_theat_a) / 6.0;psi_2 = psi_2 + hdt * (K1_psi_2 + 2.0*K2_psi_2 + 2.0*K3_psi_2 + K4_psi_2) / 6.0;omega_xi = omega_xi + hdt * (K1_omega_xi + 2.0*K2_omega_xi + 2.0*K3_omega_xi + K4_omega_xi) / 6.0;omega_Eta = omega_Eta + hdt * (K1_omega_Eta + 2.0*K2_omega_Eta + 2.0*K3_omega_Eta + K4_omega_Eta) / 6.0;omega_zeta = omega_zeta + hdt * (K1_omega_zeta + 2.0*K2_omega_zeta + 2.0*K3_omega_zeta + K4_omega_zeta) / 6.0;phi_a = phi_a + hdt * (K1_phi_a + 2.0*K2_phi_a + 2.0*K3_phi_a + K4_phi_a) / 6.0;phi_2 = phi_2 + hdt * (K1_phi_2 + 2.0*K2_phi_2 + 2.0*K3_phi_2 + K4_phi_2) / 6.0;gamma = gamma + hdt * (K1_gamma + 2.0*K2_gamma + 2.0*K3_gamma + K4_gamma) / 6.0;x = x + hdt * (K1_x + 2.0*K2_x + 2.0*K3_x + K4_x) / 6.0;y = y + hdt * (K1_y + 2.0*K2_y + 2.0*K3_y + K4_y) / 6.0;z = z + hdt * (K1_z + 2.0*K2_z + 2.0*K3_z + K4_z) / 6.0;
}
/*******************************************************************************/

（4）其它相关函数

除了上述部分外，还需要一些其它辅助函数，如：虚温随高度变化函数、空气密度函数、声速、弧度转角度、角度转弧度、读取气动数据的线性插值函数。

// 虚温随高度变化函数
double Trajectory_6FreedomDegree::tatal(double y)
{double G = 6.328e-3;if (y <= 9300) {return tatal0 - G * y;}if (9300 < y <= 12000) {return (230.2 - 6.328e-3*(y - 9300) + 1.172e-6*(y - 9300)*(y - 9300));}else if (y > 12000) {return 221.7;}
}
// 空气密度函数
double Trajectory_6FreedomDegree::rho(double P, double y)
{double tatal_temp = tatal(y);return P / (R1*tatal_temp);
}
// 声速
double Trajectory_6FreedomDegree::Cs(double y)
{double tatal_temp2 = tatal(y);return sqrt(k_gas*R1*tatal_temp2);
}
// 弧度转角度
double Trajectory_6FreedomDegree::RadToDeg(double rad)
{return rad * 180.0 / PI;
}
// 角度转弧度
double Trajectory_6FreedomDegree::DegToRad(double deg)
{return deg * PI / 180.0;
}

读取气动数据的线性插值函数的声明如下：

public:/*********************气动参数************************************/void redaData();                                      //  读取气动力系数double Interpolation_CD0(double y, double v);         //  对零升阻力系数CD0插值double Interpolation_CL_alph(double y, double v);     //  对升力系数导数CL_alphs插值double Interpolation_mz_alph(double y, double v);      //  对俯仰力矩系数导数 mz_alph插值double Interpolation_mzz_alph(double y, double v);   //  对俯仰阻尼力矩系数导数 mzz_alph插值double Interpolation_mxz_alph(double y, double v);    //  对极阻尼力矩系数导数 mxz_alph插值double Interpolation_my_alph(double y, double v);      //  对马格努斯力矩系数导数 my_alph插值double Interpolation_cz_alph(double y, double v);      //  对马格努斯力系数导数 cz_alph插值/*****************************************************************/

挑选其中的一个进行展示，例如 “对俯仰阻尼力矩系数导数 mzz_alph插值” 的定义部分如下：

double Trajectory_6FreedomDegree::Interpolation_mzz_alph(double y, double v)
{double Cs_temp = Cs(y);double ma = v / Cs_temp;for (int j = 0; j < MACHs.size(); j++) {if (MACHs[j] == ma) {return mzz_alphs[j];}else if (MACHs[j] != ma) {if (ma < MACHs[0]) {return (mzz_alphs[0] - 0) * (ma - 0) / (MACHs[0] - 0) + 0;}if (ma > MACHs.back()) {return (0 - mzz_alphs.back())*(ma - MACHs.back()) / (0 - MACHs.back()) + mzz_alphs.back();}else if (MACHs[0] < ma < MACHs.back()) {if ((ma - MACHs[j]) < (MACHs[j + 1] - MACHs[j])) {double xielv = (ma - MACHs[j]) / (MACHs[j + 1] - MACHs[j]);return (1 - xielv)*mzz_alphs[j] + xielv * mzz_alphs[j + 1];}}}}
}

（5）弹道计算结果输出函数

/*输出弹道计算结果*/// 输出的数据顺序为：时间、速度、射程、射高、横偏、弹道倾角、速度偏角、弹轴俯仰角、弹轴偏角、攻角std::ofstream outFileS_H("../result.dat", std::ios::out);for (int j = 0; j < y_s.size(); ++j){outFileS_H << t_s[j] << setw(13) << v_s[j] << setw(13) << x_s[j] << setw(13)<< y_s[j] << setw(13) << z_s[j] << setw(13) << RadToDeg(theta_a_s[j]) << setw(13) << RadToDeg(psi_2_s[j])<< setw(13) << RadToDeg(phi_a_s[j]) << setw(13) << RadToDeg(phi_2_s[j]) << setw(13) << RadToDeg(delta_s[j])<< endl;}outFileS_H.close();cout << " " << endl;cout << "6自由度刚体弹道计算结束，数据结果已输出... " << endl;

计算结束输出的结果文件 “result.dat”

3、结果展示

好了，到了这一步终于可以看看弹道程序计算完成后的结果了！！！