用龙格库塔法或欧拉法编程解算运动学弹道

import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
plt.rcParams['axes.unicode_minus'] = False
plt.rcParams['font.sans-serif'] = ['SimHei']
# 参数
c1 = -4.695
c2 = 10.735
c3 = -1.8207
c4 = 0.2623
c5 = -1.274
c6 = -0.102
v = 25  # 鱼雷匀速运动速度
vm = -5  # 目标匀速运动速度
xm0 = 1500
zm0 = 1500
beta0 = 0  # 初始角度
omega0 = 0  # 初始角速度
delta0 = 0  # 初始角加速度
psit0 = 0  # 初始航向角
Psit0 = 0  # 初始目标航向角
x0 = 0  # 初始x坐标
z0 = 0  # 初始z坐标
Psim0 = 0  # 初始目标航向角
k = 0.5
# 时间步长和模拟时间
dt = 0.01
simulation_time =100
# 初始化变量
t = 0
x = x0
z = z0
beta = beta0
omega = omega0
delta = delta0
psit = psit0
Psit = Psit0
Psim = Psim0
# 存储轨迹数据
x_vals = []
z_vals = []
psit_vals = []
# 定义函数
def q(xm, zm, x, z):
    return np.arctan2(zm - z, xm - x)

def runge_kutta_step(xm, zm, x, z, beta, omega, delta, psit, Psit, Psim, dt):
    q_val = q(xm, zm, x, z)
    psi_t = omega
    Psi_t = psi_t - beta
    eta_val = q_val - Psi_t
    delta = k * eta_val

    dw_dt = c1 * omega + c2 * beta + c3 * delta
    dbeta_dt = c4 * omega + c5 * beta + c6 * delta

    Psit = psi_t - beta - (1 - c4) * q_val - c5 * beta - c6 * delta

    dx_dt = v * np.cos(Psit)
    dz_dt = -v * np.sin(Psit)

    x += dx_dt * dt
    z += dz_dt * dt
    beta += dbeta_dt * dt
    omega += dw_dt * dt
    psit += omega * dt

    return x, z, beta, omega, psit, Psit
while t < simulation_time:
    # 调用龙格库塔法进行一步计算
    x, z, beta, omega, psit, Psit = runge_kutta_step(xm0, zm0, x, z, beta, omega, delta, psit, Psit, Psim, dt)
    x_vals.append(x)
    z_vals.append(z)
    psit_vals.append(psit)
    t += dt
    print("时间: {:.2f}, 位置: ({:.2f}, {:.2f}), 方向: {:.2f}".format(t, x, z, psit))
# 可视化
plt.figure(figsize=(10, 6))
plt.plot(x_vals, z_vals, label='鱼雷轨迹')
plt.scatter([xm0], [zm0], color='red', label='目标', marker='x')
plt.xlabel('X 坐标')
plt.ylabel('Z 坐标')
plt.title('鱼雷轨迹模拟')
plt.legend()
plt.grid()
plt.show()