航天器轨迹预测——根据速度和位置确定初轨

本文链接：https://blog.csdn.net/Tang_Zhe/article/details/122128724

一、目的

预测太空中非合作目标的轨迹

二、常见方法（航天器轨道确定方法）

利用测量数据确定轨迹：

1. 数据预处理

利用测量仪器（如雷达、多普勒测速设备、光学设备、激光测距仪等）对航天器进行跟踪观测，获取大量的位置信息及速度信息（速度信息可由位置信息推算得到）。

2. 初轨确定

利用观测数据估算轨道根数（轨道6要素）——可用于描述二体问题中的天体运动的轨迹

3. 轨道改进

利用大量的观测数据修正初轨以得到精确的轨道根数

三、初轨确定——根据速度、位置推算六根数

前提假设：

二体问题，eg：已知近地卫星绕地球飞行，求卫星轨迹
假设轨迹为圆锥曲线，包括椭圆、抛物线、双曲线、圆（暂不考虑突然的加减速）

获取卫星的运动轨迹问题可等效于求轨道六根数。

轨道六根数可表征轨道形状、位置、运动特征。

轨道六根数包括：半长轴a、离心率e、轨道倾角i、近心点辐角ω、升交点经度Ω、真近点角φ

半长轴a、离心率e：确定轨道形状
轨道倾角i、近心点辐角ω、升交点经度Ω：确定轨道平面所处的位置
真近点角φ：确定卫星在轨道中的位置

已知卫星的速度矢量和位置矢量可以求六根数，已知六根数也可求出其位置和速度。

1. 推导过程

已知：

速度矢量 $\widehat{v}$
到中心天体的位置矢量 $\widehat{r}$
x、y、z轴单位矢量 $\widehat{x}$ 、 $\widehat{y}$ 、 $\widehat{z}$
中心天体引力常数μ

中心天体引力常数 $\mu$ = 万有引力常数 $G$ * 中心天体的质量 $M$

万有引力常数推荐值 $G=6.67259*10^{-11} N\cdot m^{2}/kg^{2}$

地球质量 $M=5.974*10^{24} kg$

地球的中心天体引力常数 $\mu = 3.986*10^{14} m^3 \cdot s^{-2}$

求：

1）半长轴a

$a=\left ( \frac{2}{r} - \frac{v^{2}}{\mu} \right )^{-1}$

若a为有限正数，则轨道为椭圆；
若a为负数，则为双曲线，取a=|a|；
若a为无穷数（分母为0），表明是抛物线，抛物线没有半长轴，计算半通径：

角动量 $\widehat{h} = \widehat{r} \times \widehat{v}$

$a = \frac{h^{2}}{\mu}$

2）离心率e

$\widehat{e} = \frac{1}{\mu} [(v^{2} - \frac{\mu}{r})\widehat{r} - (\widehat{r}\cdot \widehat{v})\widehat{v}]$

六根数中的离心率为离心率矢量的模。

若e为0，则轨道为圆

3）轨道倾角i

$i=arccos (\frac{\widehat{z} \cdot \widehat{h}}{h})$

4）近心点辐角ω

升交线矢量 $\widehat{n} = \widehat{z} \times \widehat{h}$

圆形轨道，没有近心点幅角，计算升交点离角：

$\omega = arccos( \frac{\widehat{n} \cdot \widehat{r}}{nr})$

如果 $\widehat{z} \cdot \widehat{r} > 0$ ，即位置矢量与z轴正半轴夹角为锐角，那么ω<180

非圆形轨道，可按如下公式计算：

$\omega = arccos( \frac{\widehat{n} \cdot \widehat{e}}{ne})$

如果 $\widehat{z} \cdot \widehat{e} > 0$ ，即离心率矢量与z轴正半轴夹角为锐角，那么ω<180

5）升交点经度Ω

$\Omega =arccos( \frac{\widehat{n} \cdot \widehat{x}}{n})$

若 $\widehat{y} \cdot \widehat{n} > 0$ ，即升交线与y轴交角为锐角，则Ω<180

6）真近点角φ

圆形轨道，不计算φ
非圆形轨道，可按如下公式计算：

$\varphi = arcos(\frac{\widehat{e} \cdot \widehat{r}}{er})$

若 $\widehat{r} \cdot \widehat{v} > 0$ ，即位置矢量与速度矢量之间的夹角为锐角，则φ<180

2. python实现

问题描述：

二体问题，中心天体为地球
轨道为圆锥曲线（目前只写了椭圆形轨道）
已知卫星的真实位置和速度，求卫星轨道（初轨），并计算误差（根据六根数确定的当前点与实际当前点之间的距离）
数据来源：https://scihub.copernicus.eu/gnss/#/home 哨兵1B的精密轨道数据（用户名/密码：gnssguest）

# 二体问题
# 假设轨道为圆锥曲线，即：椭圆/抛物线/双曲线/圆
# 用轨道六根数，描述圆锥曲线

import math
import numpy as np
import matplotlib.pyplot as plt
import xml.etree.ElementTree as ET
import os
from CommonFunc import rotate_3D, mkdir_deep, comp_round

save_path_prefix = './Route/infos'
miu = 3.986 * 1e14 # 地球的万有引力常数（精确到后四位及以上对误差影响不大）
f = [] # 真实轨迹数据文件
x_real, y_real, z_real = [], [], [] # 真实轨迹数据的xyz值

def read_route(save_path=None):
    # 读取哨兵的精密轨道数据
    read_path = '/home/tangzhe/data/trajectory/S1B_OPER_AUX_RESORB_OPOD_20211229T060918_V20211229T020523_20211229T052253.EOF'
    if not save_path:
        save_path = os.path.join(save_path_prefix, 'data.npy')
    tree = ET.parse(read_path)
    root = tree.getroot()
    Data_Block = root.find('Data_Block')
    if not Data_Block:
        print('ERROR! cannot find Data_Block')
        return
    List_of_OSVs = Data_Block.find('List_of_OSVs')
    if not List_of_OSVs:
        print('ERROR! cannot find List_of_OSVs')
        return
    # 获取所有的位置及速度
    n = len(List_of_OSVs)
    data = np.zeros((6,n), dtype=np.float64)
    for i in range(n):
        data[0][i] = float(List_of_OSVs[i].find('X').text)
        data[1][i] = float(List_of_OSVs[i].find('Y').text)
        data[2][i] = float(List_of_OSVs[i].find('Z').text)
        data[3][i] = float(List_of_OSVs[i].find('VX').text)
        data[4][i] = float(List_of_OSVs[i].find('VY').text)
        data[5][i] = float(List_of_OSVs[i].find('VZ').text)
    plot_traj(data[0], data[1], data[2], save_path=os.path.join(save_path_prefix, 'real_path.png')) # 绘制真实轨迹
    # 保存数据
    np.save(save_path, data)

def comp_oe(R, V):
    # 已知位置、速度，求六根数
    # input:
    # R:位置矢量
    # V:速度矢量
    # output:
    # [a, e, i, w, Omega, phi]
    # a半长轴;e离心率；i轨道倾角；Omega升交点经度；phi真近点角

    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]

def comp_traj(OE):
    # 计算轨道的参数方程
    [a, e, i, w, Omega, phi] = OE
    preci = 4 # 角度刻度精确到小数点后几位
    theta = np.linspace(0, 2*np.pi, pow(10, preci))
    b = a*math.sqrt(1 - e*e)
    # 原始曲线曲线（位于xy平面内）
    x = a*np.cos(theta)
    y = b*np.sin(theta)
    z = np.zeros(len(theta), dtype=np.float64)
    # 根据六根数旋转曲线
    x, y, z = rotate_3D(x, y, z, i, [0, 1, 0]) # 绕y轴旋转i度
    x, y, z = rotate_3D(x, y, z, w - np.pi/2, [1, 0, 0]) # 绕x轴旋转w-pi/2度
    x, y, z = rotate_3D(x, y, z, Omega - np.pi/2, [0, 0, 1]) # 绕z轴旋转Omega-pi/2度
    # 根据phi确定当前点
    angle = phi + np.pi
    if angle > 2*np.pi: angle -= 2*np.pi
    idx = np.where(theta >= angle)[0][0]
    pos_comp = np.array([x[idx], y[idx], z[idx]])
    # plot_traj(x, y, z, pos_comp=pos_comp)
    return x, y, z, pos_comp

def plot_traj(x, y, z, pos_comp=[], pos_real=[], save_path=None):
    # 绘制轨迹
    fig = plt.figure()
    ax = fig.add_subplot(projection='3d', title='Trajectory')
    ax.plot(x, y, z, color='blue', linestyle='-')
    ax.plot(x_real, y_real, z_real, color='r', linestyle='--')
    if len(pos_comp):
        ax.scatter(pos_comp[0], pos_comp[1], pos_comp[2], color='b', marker='o')
    if len(pos_real): # 绘制真实轨迹点
        ax.scatter(pos_real[0], pos_real[1], pos_real[2], color='r', marker='o')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    if not save_path:
        save_path = os.path.join(save_path_prefix, 'test.png')
    plt.savefig(save_path)
    # plt.show()
    plt.close()

def comp_once(idx, save_path=None):
    # 计算一次轨迹和误差
    R = np.array([f[0][idx], f[1][idx], f[2][idx]])
    V = np.array([f[3][idx], f[4][idx], f[5][idx]])
    OE = comp_oe(R, V)
    # print('OE:', OE)
    x, y, z, pos_comp = comp_traj(OE)
    if save_path:
        plot_traj(x, y, z, pos_comp=pos_comp, pos_real=R, save_path=save_path)
    error = np.linalg.norm(pos_comp - R)
    return error

if __name__ == '__main__':
    mkdir_deep(save_path_prefix)
    # 处理输入数据
    real_data_save_path = os.path.join(save_path_prefix, 'data.npy')
    # read_route(real_data_save_path) # 读取原始数据
    f = np.load(real_data_save_path)
    x_real, y_real, z_real = f[0], f[1], f[2]

    # 计算误差
    error_lst = [comp_once(i) for i in range(len(f[0]))]
    # 分析误差
    _ = comp_once(error_lst.index(min(error_lst)), save_path=os.path.join(save_path_prefix, 'min_error.png'))
    _ = comp_once(error_lst.index(max(error_lst)), save_path=os.path.join(save_path_prefix, 'max_error.png'))
    print('min error =', comp_round(min(error_lst)))
    print('max error =', comp_round(max(error_lst)))
    print('ave error =', comp_round(np.average(error_lst)))

# 误差分析：
# 精密轨道数据的误差：5cm
# min error = 137682
# max error = 2156700
# ave error = 1289524