航天器状态(位置速度)轨道六根数

有三种方法，第一种和第三种在求椭圆和近圆轨道参数时是正确的，第二种方法不太精确。其余轨道我没有仔细测过，你们可以自己试一试。

def Coordinate_Classic(R, V, miu):
    # 经典坐标：将速度和位置转换为轨道因素
    # miu：中心天体的 GM
    # data：对于椭圆和双曲线：a;e;i;w;W;fai 的转移轨道
    # %a：半长轴；e：离心率；i：倾角；
    # %w：近地点幅角；W：升交点经度；fai：真近点角。
    # 对于抛物线：p;i;w;W;fai 的转移轨道
    # %p：半矩形矩；i：倾角；w：近地点幅角；
    # %W：升交点经度；fai：真近点角。
    # 对于圆：a;i;u;W; 的转移轨道
    # %a：半径；i：倾角；u：纬度参数；
    # %W：升交点经度；
    # 坐标：位置
    # V：速度

    # 计算位置矢量的模长
    r = np.sqrt(np.dot(R, R))
    # 如果速度矢量的模长与两倍位置矢量的模长之差不为零
    if 2/r - np.dot(V, V)/miu != 0:
        # 计算椭圆轨道的半长轴
        a = 1/abs(2/r - np.dot(V, V)/miu)
    else:
        a = None

    # 计算能量矢量
    E = (np.dot(V, V)/miu - 1/r) * R - np.dot(R, V)/miu * V
    # 计算轨道离心率
    e = np.sqrt(np.dot(E, E))

    # 计算角动量矢量
    H = np.cross(R, V)
    # 计算角动量的模长
    h = np.linalg.norm(H)
    # 计算抛物线轨道的半矩形矩
    p = h**2 / miu

    Z = np.array([0, 0, 1])
    X = np.array([1, 0, 0])
    Y = np.array([0, 1, 0])
    N = np.cross(Z, H)
    # 计算升交点赤经的单位矢量
    n = np.linalg.norm(N)
    # 计算倾角
    i = np.arccos(np.dot(Z, H)/h)

    # 如果轨道离心率不为零
    if e != 0:
        # 计算近地点幅角
        w = np.arccos(np.dot(N, E)/n/e)
        if np.dot(Z, E) < 0:
            w = 2*np.pi - w
    else:
        # 计算纬度参数
        u = np.arccos(np.dot(N, R)/n/r)
        if np.dot(R, Z) < 0:
            u = 2*np.pi - u

    # 计算升交点赤经
    W = np.arccos(np.dot(X, N)/n)
    if np.dot(Y, N) < 0:
        W = 2*np.pi - W

    # 如果轨道离心率不为零
    if e != 0:
        # 计算真近点角
        fai = np.arccos(np.dot(E, R)/e/r)
        if np.dot(R, V) < 0:
            fai = 2*np.pi - fai

    # 如果速度矢量的模长与两倍位置矢量的模长之差不为零
    if 2/r - np.dot(V, V)/miu != 0:
        # 如果轨道离心率不为零
        if e != 0:
            data = [a, e, i, w, W, fai]
        else:
            data = [a, i, u, W]
    else:
        data = [p, i, w, W, fai]

    return data

def R0_Classic(r, v):
    # mu = 398600.4418
    mu = 3.986e14
    r_norm = np.linalg.norm(r)
    v_norm = np.linalg.norm(v)
    r_dot_v = np.dot(r, v)

    # 计算特征矢量
    h = np.cross(r, v)
    h_norm = np.linalg.norm(h)

    # 计算偏心率矢量
    e_vec = ((v_norm ** 2 - mu / r_norm) * r - r_dot_v * v) / mu
    e = np.linalg.norm(e_vec) # 计算偏心率

    # 计算半长轴
    a = 1 / (2 / r_norm - v_norm ** 2 / mu)

    # 计算轨道倾角
    i = np.arccos(h[2] / h_norm)

    # 计算近地点幅角
    omega = np.arccos(np.dot(np.array([1, 0, 0]), e_vec / e))
    if e_vec[2] < 0:
        omega = 2 * np.pi - omega

    # 计算升交点赤经
    Omega = np.arccos(h[0] / h_norm / np.sin(i))
    if h[1] < 0:
        Omega = 2 * np.pi - Omega

    # 计算真近点角
    f = np.arccos(np.dot(e_vec, r) / e / r_norm)
    if r_dot_v < 0:
        f = 2 * np.pi - f

    data = [a, e, i, omega, Omega, f]
    return data

def comp_oe(R, V):
    X = [1, 0, 0]  # y轴方向向量
    Y = [0, 1, 0]  # y轴方向向量
    Z = [0, 0, 1]  # z轴方向向量
    r = np.linalg.norm(R)  # 位置标量
    H = np.cross(R, V)  # 角动量
    h = np.linalg.norm(H)  # 角动量的模
    N = np.cross(Z, H)  # 升交线矢量
    n = np.linalg.norm(N)  # 升交线矢量的模
    # 半长轴 a
    tmp = 2 / r - np.dot(V, V) / miu
    if tmp == 0:  # 抛物线
        a = np.dot(H, H) / miu
    else:
        a = abs(1 / tmp)
    # 离心率 e
    E = ((np.dot(V, V) - miu / r) * R - np.dot(R, V) * V) / miu  # 离心率矢量
    e = np.linalg.norm(E)  # 离心率标量
    if e < 1e-7: e = 0
    # 轨道倾角 i
    i = math.acos(np.dot(Z, H) / h)
    # 近心点辐角 w
    if e == 0:  # 圆
        w = math.acos(np.dot(N, R) / (n * r))
        if np.dot(Z, R) < 0:
            w = 2 * np.pi - w
    else:
        w = math.acos(np.dot(N, E) / (n * e))
        if np.dot(Z, E) < 0:
            w = 2 * np.pi - w
    # 升交点经度 Omega
    Omega = math.acos(np.dot(N, X) / n)
    if np.dot(N, Y) < 0:
        Omega = 2 * np.pi - Omega
    # 真近点角 phi
    if e != 0:  # 非圆形轨道
        phi = math.acos(np.dot(E, R) / (e * r))
        if np.dot(R, V) < 0:
            phi = 2 * np.pi - phi
    else:
        phi = 0
    return [a, e, i, w, Omega, phi]

我的输入信息为

R0 = np.array([2000000, 2000000 ,1000000])
V0 = np.array([1710, 1140, 1300])
miu = 3.986e14
data1 = Coordinate_Classic(R0, V0, miu)
data2 = R0_Classic(R0, V0)
data3 = comp_oe(R0, V0)