使用Heun法数值积分扁球体地球卫星运动方程时出现无效值警告与空图问题求助

我现在在用Heun方法数值积分扁球体地球周围卫星的运动方程，但运行时总是弹出RuntimeWarning: invalid value encountered in scalar divide的警告，而且画出来的图是空的——只有坐标轴，完全看不到轨道曲线。我本来期望能得到一个类似椭圆、围绕原点的卫星轨道。

问题背景 #

我要解的是考虑J2摄动的卫星运动哈密顿方程，变量包括径向距离r、纬度phi、经度th，以及对应的共轭动量p_r、p_phi、p_th。

我的实现代码 #

from math import sin, cos, pi
import numpy as np
from matplotlib import pyplot as plt

G = 6.67e-11
M = 5.97e24
m = 3000
R = 6371000
J2 = 0.001341125
dt = 0.1

t = np.linspace(0,500,5000)

r = np.empty(len(t))
phi = np.empty(len(t))
th = np.empty(len(t))
p_r = np.empty(len(t))
p_phi = np.empty(len(t))
p_th = np.empty(len(t))

def f(r,phi,th,p_r,p_phi,p_th):
    return p_r/m

def g(r,phi,th,p_r,p_phi,p_th):
    return p_phi**2/(m*r**3) + p_th**2/(m*r**3*cos(phi)**2) - G*M*m/r**2 + 3*J2*G*M*m*R**2*(3*sin(phi)**2 - 1)/(2*r**4)

def h(r,phi,th,p_r,p_phi,p_th):
    return p_phi/(m*r**2)

def j(r,phi,th,p_r,p_phi,p_th):
    return -p_th**2*sin(phi)/(m*r**2*cos(phi)**3) - 3*J2*G*M*m*R**2*sin(phi)*cos(phi)/r**3

def k(r,phi,th,p_r,p_phi,p_th):
    return p_th/(m*r**2*cos(phi)**2)

def Heun(f,g,h,j,k,t):
    # 初始条件
    r[0] = 1.4371e7
    phi[0] = 0.49672
    th[0] = -1.4055
    p_r[0] = 1.1775e7
    p_phi[0] = 1.744e14
    p_th[0] = 5.3921e14

    rp = np.empty(len(t))
    phip = np.empty(len(t))
    thp = np.empty(len(t))
    p_rp = np.empty(len(t))
    p_phip = np.empty(len(t))
    p_thp = np.empty(len(t))

    for n in range(len(t) - 1):
        # Heun预测步
        p_rp[n+1] = p_r[n] + dt*g(r[n],phi[n],th[n],p_r[n],p_phi[n],p_th[n])
        rp[n+1] = r[n] + dt*p_r[n]/m
        p_phip[n+1] = p_phi[n] + dt*j(r[n],phi[n],th[n],p_r[n],p_phi[n],p_th[n])
        phip[n+1] = phi[n] + dt*p_phi[n]/(m*r[n]**2)
        p_thp[n+1] = p_th[n]
        thp[n+1] = th[n] + dt*k(r[n],phi[n],th[n],p_r[n],p_phi[n],p_th[n])

        # Heun校正步
        p_r[n+1] = p_r[n] + dt/2*(g(r[n],phi[n],th[n],p_r[n],p_phi[n],p_th[n]) + g(rp[n],phip[n],thp[n],p_rp[n],p_phip[n],p_thp[n]))
        r[n+1] = r[n] + dt/2*(p_r[n]/m + p_rp[n]/m)
        p_phi[n+1] = p_phi[n] + dt/2*(j(r[n],phi[n],th[n],p_r[n],p_phi[n],p_th[n]) + j(rp[n],phip[n],thp[n],p_rp[n],p_phip[n],p_thp[n]))
        phi[n+1] = phi[n] + dt/2*(h(r[n],phi[n],th[n],p_r[n],p_phi[n],p_th[n]) + h(rp[n],phip[n],thp[n],p_rp[n],p_phip[n],p_thp[n]))
        p_th[n+1] = p_th[n]
        th[n+1] = th[n] + dt/2*(k(r[n],phi[n],th[n],p_r[n],p_phi[n],p_th[n]) + k(rp[n],phip[n],thp[n],p_rp[n],p_phip[n],p_thp[n]))

        # 尝试避免phi接近pi/2导致除零
        if phi[n+1] == pi/2:
            break
    return t,r,phi,th,p_r,p_phi,p_th

t,r,phi,th,p_r,p_phi,p_th = Heun(f,g,h,j,k,t)

# 转换为笛卡尔坐标绘图
x = r*np.cos(phi)*np.cos(th)
y = r*np.cos(phi)*np.sin(th)
z = r*np.sin(phi)

fig = plt.figure()
ax = plt.axes(projection='3d')
ax.plot3D(x, y, z, 'blue')
plt.show()

我怀疑的问题点 #

1. 警告里的除零错误大概率来自cos(phi)相关项：当phi接近pi/2时，cos(phi)趋近于0，会引发分母为0的计算错误。我加了if phi[n+1] == pi/2: break的判断，但似乎没起作用——可能变量在更早的迭代中就已经出现了NaN？
2. Heun方法的实现会不会有问题？比如预测步和校正步的变量更新顺序、初始条件的处理是否正确？

有没有大佬能帮我定位问题根源，给出解决办法，让轨道图正常显示出来？

内容来源于stack exchange