【C++】利用Eigen库实现弹道计算，并输出弹道轨迹
发布时间
阅读量：
阅读量
利用Eigen库实现弹道计算，并输出弹道轨迹代码
弹道知识简介:
**弹道导弹**是指在火箭发动机推力作用下按预定程序飞行，关机后按自由抛物体轨迹飞行的导弹。其飞行弹道一般分为主动段和被动段：主动段（又称动力飞行段或助推段）是导弹在火箭发动机推力和制导系统作用下，从发射点起飞到火箭发动机关机时的飞行路径；被动段包括自由飞行段和再入段，是导弹按照在主动段终点获得的给定速度和弹道仪角作惯性飞行，到弹头起爆的路径。
AI助手 常规弹箭在空中飞行时，除了弹道导弹弹道主动段受到推力作用外，全部飞行过程仅考虑重力和空气阻力的弹体质心运动规律，属于质点弹道研究的范围。不考虑空气动力影响的弹道称为真空弹道；单纯考虑恒定重力的真空弹道为抛物线；同时考虑重力大小和方向变化及地球表面曲率影响的远程真空弹道则为椭圆曲线。在地球表面固连坐标系内研究远程弹箭的运动时，需要计及地球表面曲率和地球自转所引起的科里奥利力对射程和方向的影响。考虑空气动力影响的弹道称为空气弹道。由于空气动力矢量线一般不通过弹体质心，因而空气动力和力矩同时作用于弹体，这不仅产生了质心运动，而且具有围绕质心（简称绕心）运动。绕心运动对质心运动的影响，使弹箭质心运动轨迹变为一条复杂的空间螺线，它属于刚体弹道的研究内容。
AI助手 在外弹道学中，对轴对称的弹箭气动力及其系数的描述，更加突出了气动力各分量的作用性质和物理概念。空气动力分解为阻力、升力和马格努斯力，空气动力矩分解为静力矩、赤道阻尼力矩（横向摆动阻尼力矩）、尾翼导转力矩、滚转阻尼力矩及马格努斯力矩等。在弹箭控制飞行过程中，还要受到控制力和力矩作用。全面计及诸力和力矩作用下所建立的弹箭运动方程组，称为全力组外弹道模型。
AI助手 由于涉及空气动力学太过复杂，所以本算法仅考虑重力基本条件进行粗略弹道计算。另外，eigen库可在github上自行下载。
AI助手代码如下：
/**************************************************** *
*@Copyright (c) 2024, GhY, All rights reserved.
*@文件 public.h
*@描述 公共数据定义
* *@作者 GhY
*@日期 2024年7月29日
*@版本 v1.0.0
* ****************************************************/
#pragma once
constexpr double pi = 3.1415926535897932;
constexpr double deg_per_rad = 180 / pi;
constexpr double rad_per_deg = pi / 180;
// 地心坐标系(WGS84)定义的一些常数
constexpr double a = 6378137;
constexpr double a2 = a * a;
constexpr double ra2 = 1 / a2;
constexpr double squash = 1 - 1 / 298.257223563;
constexpr double strech = 1 / squash;
constexpr double squash2 = squash * squash;
constexpr double e2 = 1 - squash2;
constexpr double e4 = e2 * e2;
/* *@brief 大地坐标
*@author GhY
*@date 2024/07/29
*/
struct PosLLA {
/** \brief 经度（度） */
double Lon;
/** \brief 纬度（度） */
double Lat;
/** \brief 高（米） */
double Alt;
PosLLA()
{
Lon = 0.0;
Lat = 0.0;
Alt = 0.0;
}
PosLLA(double lon, double lat, double hight)
: Lon(lon), Lat(lat), Alt(hight) {}
bool operator==(const PosLLA& o) const
{
return this->Lon == o.Lon && this->Lat == o.Lat && this->Alt == o.Alt;
}
bool operator!=(const PosLLA& o) const
{
return !(*this == o);
}
};
/* *@brief 角度转弧度
*@author GhY
*@date 2024/07/29
*/
static inline constexpr double DegConvertRad(double deg)
{
return deg * rad_per_deg;
}
/* *@brief 弧度转角度
*@author GhY
*@date 2024/07/29
*/
static inline constexpr double RadConvertDeg(double rad)
{
return rad * deg_per_rad;
}
AI助手 /**************************************************** *
*@Copyright (c) 2024, GhY, All rights reserved.
*@文件 MissileTrajCalculate.h
*@描述 弹道计算类 声明
* *@作者 GhY
*@日期 2024年7月29日
*@版本 v1.0.0
* ****************************************************/
#pragma once
#include "public.h"
#include "Eigen/Dense"
#include <cmath>
using namespace Eigen;
class MissileTrajCalculate
{
public:
MissileTrajCalculate();
~MissileTrajCalculate();
static Vector3d GeodConvertCart(double L, double B, double H);
static PosLLA CartConvertGeod(const Vector3d& cart);
/** *@brief 计算弹道
*@param[in] L0 发射点经度（度）
*@param[in] B0 发射点纬度（度）
*@param[in] L1 落点经度（度）
*@param[in] B1 落点纬度（度）
*@param[in] velocity 导弹发射速度（米/秒）
*@param[in] maxHeight 弹道最大高度（米）
*@param[in] stepSize 步长（秒）
*@param[inout] len 内存分配的out数组长度、输出结果长度
*@param[out] flyTime 飞行时间（秒）
*@param[out] out 输出结果
*@retval 数组内存分配长度是否足够存放输出结果
*/
bool CalculateTrajectory(
double L0, double B0, double L1, double B1,
double velocity, double maxHeight, double stepSize,
int* len, double* flyTime, PosLLA out[]);
};
AI助手 /**************************************************** *
*@Copyright (c) 2024, GhY, All rights reserved.
*@文件 MissileTrajCalculate.h
*@描述 弹道计算类 实现
* *@作者 GhY
*@日期 2024年7月29日
*@版本 v1.0.0
* ****************************************************/
#include "MissileTrajCalculate.h"
MissileTrajCalculate::MissileTrajCalculate()
{
}
MissileTrajCalculate::~MissileTrajCalculate()
{
}
Vector3d MissileTrajCalculate::GeodConvertCart(double L, double B, double H)
{
double lambda = DegConvertRad(L);
double phi = DegConvertRad(B);
double sphi = sin(phi);
double sphi2 = sphi * sphi;
double cphi = sqrt(1 - sphi2);
double n = a / sqrt(1 - e2 * sphi2);
double s = (H + n) * cphi;
double X = s * cos(lambda);
double Y = s * sin(lambda);
double Z = (H + squash2 * n) * sphi;
return Vector3d(X, Y, Z);
}
PosLLA MissileTrajCalculate::CartConvertGeod(const Vector3d& cart)
{
PosLLA res;
double X = cart(0);
double Y = cart(1);
double Z = cart(2);
double X2pY2 = X * X + Y * Y;
double ZZ = Z * Z;
if (X2pY2 + ZZ < 25) {
res.Lon = 0;
res.Lat = 0;
res.Alt = -a;
return res;
}
double sqrtX2pY2 = sqrt(X2pY2);
double p = X2pY2 * ra2;
static const double f = squash2 * ra2;
double q = ZZ * f;
double r = 1 / 6.0 * (p + q - e4);
double s = e4 * p * q / (4 * r * r * r);
/*
当s=[-2..0]时，由于浮点舍入误差，s*（2+s）值略为负值
所以将这个结果限定在正数范围内。
*/
if (s >= -2.0 && s <= 0.0) {
s = 0.0;
}
double t = pow(1 + s + sqrt(s * (2 + s)), 1 / 3.0);
double u = r * (1 + t + 1 / t);
double v = sqrt(u * u + e4 * q);
double w = e2 * (u + v - q) / (2 * v);
double k = sqrt(u + v + w * w) - w;
double D = k * sqrtX2pY2 / (k + e2);
double sqrtD2pZ2 = sqrt(D * D + ZZ);
double lambda = atan(Y / X);
if (lambda != lambda) {
lambda = 0;
} else if (X < 0) {
if (Y >= 0) {
lambda += pi;
} else {
lambda -= pi;
}
}
res.Lon = RadConvertDeg(lambda);
res.Lat = RadConvertDeg(atan(Z / D));
res.Alt = (k + e2 - 1) * sqrtD2pZ2 / k;
return res;
}
bool MissileTrajCalculate::CalculateTrajectory(double L0, double B0,
double L1, double B1, double velocity, double maxHeight,
double stepSize, int* len, double* flyTime, PosLLA out[])
{
Vector3d P0 = GeodConvertCart(L0, B0, 0);
Vector3d P1 = GeodConvertCart(L1, B1, 0);
double d = 0.5 * (P0 - P1).norm();
double d2 = d * d;
double h = a - sqrt(a2 - d2) + maxHeight;
double r = 0.5 * (d2 / h + h);
double dist = 2 * asin(d / r) * r;
*flyTime = dist / velocity;
int size = *flyTime / stepSize;
if (*len < size) {
*len = size;
return false;
}
*len = size;
Vector3d N = P0.cross(P1).normalized();
Vector3d C = 0.5 * (P0 + P1) + (r - h) * N.cross(P1 - P0).normalized();
Vector3d R0 = P0 - C;
double delta = velocity * stepSize / r;
double theta = 0;
for (int i = 0; i < size; i++) {
theta += delta;
Vector3d P = AngleAxisd(theta, N) * R0 + C;
out[i] = CartConvertGeod(P);
}
return true;
}
AI助手测试代码：
#include<stdio.h>
#include <vector>
#include "MissileTrajCalculate.h"
int main()
{
MissileTrajCalculate mtc;
int len = 2000; // 给定一个输出坐标个数，实际数量以返回结果为准
double time = 0;
PosLLA data[2000];
PosLLA stPos(121.857917, 30.963701, 0); // 发射点
PosLLA enPos(130.866225, 30.882431, 0); // 目标点
bool bRet = mtc.CalculateTrajectory(stPos.Lon, stPos.Lat, enPos.Lon, enPos.Lat, 400, 30000, 10, &len, &time, data);
if (!bRet) {
printf("calculate trajectory failure......");
return 1;
}
std::vector<PosLLA> vecLLAs;
vecLLAs.push_back(stPos); //添加起点
int num = 1;
for (auto d : data) {
if (num > len) {
break;
}
PosLLA lla;
lla.Lon = d.Lon;
lla.Lat = d.Lat;
lla.Alt = d.Alt;
vecLLAs.push_back(lla);
num++;
}
vecLLAs.push_back(enPos); // 添加终点
int cout = 0;
for (auto lla : vecLLAs) {
printf("num: %d \t Lon: %lf \t Lat: %lf \n", cout, lla.Lon, lla.Lat);
cout++;
}
getchar();
return 0;
}
AI助手输出结果:
全部评论 (0)
还没有任何评论哟~