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

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

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 / miuZ = 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 - welse:# 计算纬度参数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.4418mu = 3.986e14r_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) / mue = 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 - fdata = [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)  # 升交线矢量的模# 半长轴 atmp = 2 / r - np.dot(V, V) / miuif tmp == 0:  # 抛物线a = np.dot(H, H) / miuelse:a = abs(1 / tmp)# 离心率 eE = ((np.dot(V, V) - miu / r) * R - np.dot(R, V) * V) / miu  # 离心率矢量e = np.linalg.norm(E)  # 离心率标量if e < 1e-7: e = 0# 轨道倾角 ii = math.acos(np.dot(Z, H) / h)# 近心点辐角 wif e == 0:  # 圆w = math.acos(np.dot(N, R) / (n * r))if np.dot(Z, R) < 0:w = 2 * np.pi - welse:w = math.acos(np.dot(N, E) / (n * e))if np.dot(Z, E) < 0:w = 2 * np.pi - w# 升交点经度 OmegaOmega = math.acos(np.dot(N, X) / n)if np.dot(N, Y) < 0:Omega = 2 * np.pi - Omega# 真近点角 phiif e != 0:  # 非圆形轨道phi = math.acos(np.dot(E, R) / (e * r))if np.dot(R, V) < 0:phi = 2 * np.pi - phielse:phi = 0return [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)