弹箭六自由度弹道计算程序(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/K
	const 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*m2
	double 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、结果展示

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

4、气动计算

我们在算六自由度弹道的时候需要有飞行物体的气动力和气动力矩系数，《missiledatcom算气动详解》 我写的这篇文章里面对算气动的missiledatcom进行了详细介绍，并且在《missiledatcom算气动详解》 这篇文章的末尾还附带了我的联系方式。
欢迎沟通交流哦~