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

一、目的

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

二、常见方法(航天器轨道确定方法)

利用测量数据确定轨迹:

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

from CommonFunc import rotate_3D, mkdir_deep, comp_round

CommonFunc :

Common Function - Python

  • 8
    点赞
  • 80
    收藏
    觉得还不错? 一键收藏
  • 8
    评论
以下是利用开普勒方程递推轨道的MATLAB程序: %% 利用开普勒方程递推轨道程序 clear all;clc; %% 输入参数 mu = 398600.4418; % 地球引力常数 a = 7000; % 初轨道长半轴 e = 0.0; % 初轨道离心率 i = 30*pi/180; % 初轨道倾角 RAAN = 0; % 升交点赤经 omega = 0; % 近地点幅角 M0 = 0; % 初平近点角 %% 计算轨道参数 n = sqrt(mu/a^3); % 平均角速度 T = 2*pi/n; % 周期 E0 = M0; % 初偏近点角 while 1 E = M0 + e*sin(E0); % 开普勒方程 if abs(E - E0) < 1e-8 % 判断E是否收敛 break; end E0 = E; % 更新E0 end theta = 2*atan(sqrt((1+e)/(1-e))*tan(E/2)); % 真近点角 h = sqrt(mu*a*(1-e^2)); % 轨道角动量 p = a*(1-e^2); % 焦距 r = p/(1+e*cos(theta)); % 距离 v = sqrt(2*(E+mu/r)); % 速度 r_dot = sqrt(mu*p)/r*v*sin(theta); % 距离变化率 r_theta_dot = h/r^2; % 弧速度 r_cross_v = [0,0,r*r_theta_dot]; % 距离矢量与速度矢量的叉积 v_cross_h = cross([0,0,h], [r*cos(theta),r*sin(theta),0]); % 速度矢量与角动量矢量的叉积 e_vec = 1/mu*((v^2-mu/r)*[r*cos(theta),r*sin(theta),0]-r_dot*[0,0,r]-r_cross_v); % 离心率矢量 i_vec = [cos(RAAN)*cos(omega)-sin(RAAN)*sin(omega)*cos(i), sin(RAAN)*cos(omega)+cos(RAAN)*sin(omega)*cos(i), sin(omega)*sin(i)]; % 轨道面法向量 n_vec = cross([0,0,1], i_vec); % 升交点赤道面法向量 h_vec = [r*sqrt(v^2-(r_dot/r)^2)*sin(theta),-r*sqrt(v^2-(r_dot/r)^2)*cos(theta),h]; % 角动量矢量 RAAN_dot = n/h_vec(3); % 升交点赤经变化率 omega_dot = dot(e_vec, n_vec)/(e*h); % 近地点幅角变化率 i_dot = dot(h_vec, cross(n_vec, e_vec))/h; % 倾角变化率 %% 递推计算 t = 0; % 初始时间 dt = 60; % 时间步长 M = M0 + n*t; % 平近点角 while M < 2*pi % 递推直到一圈结束 E0 = M; % 初偏近点角 while 1 E = M + e*sin(E0); % 开普勒方程 if abs(E - E0) < 1e-8 % 判断E是否收敛 break; end E0 = E; % 更新E0 end theta = 2*atan(sqrt((1+e)/(1-e))*tan(E/2)); % 真近点角 r = p/(1+e*cos(theta)); % 距离 v = sqrt(2*(E+mu/r)); % 速度 r_dot = sqrt(mu*p)/r*v*sin(theta); % 距离变化率 r_theta_dot = h/r^2; % 弧速度 r_cross_v = [0,0,r*r_theta_dot]; % 距离矢量与速度矢量的叉积 v_cross_h = cross([0,0,h], [r*cos(theta),r*sin(theta),0]); % 速度矢量与角动量矢量的叉积 e_vec = 1/mu*((v^2-mu/r)*[r*cos(theta),r*sin(theta),0]-r_dot*[0,0,r]-r_cross_v); % 离心率矢量 i_vec = [cos(RAAN)*cos(omega)-sin(RAAN)*sin(omega)*cos(i), sin(RAAN)*cos(omega)+cos(RAAN)*sin(omega)*cos(i), sin(omega)*sin(i)]; % 轨道面法向量 n_vec = cross([0,0,1], i_vec); % 升交点赤道面法向量 h_vec = [r*sqrt(v^2-(r_dot/r)^2)*sin(theta),-r*sqrt(v^2-(r_dot/r)^2)*cos(theta),h]; % 角动量矢量 RAAN = RAAN + RAAN_dot*dt; % 更新升交点赤经 omega = omega + omega_dot*dt; % 更新近地点幅角 i = i + i_dot*dt; % 更新倾角 t = t + dt; % 更新时间 M = M0 + n*t; % 平近点角 end %% 输出结果 fprintf('轨道长半轴a = %.2f km\n', a); fprintf('轨道离心率e = %.2f\n', e); fprintf('轨道倾角i = %.2f deg\n', i*180/pi); fprintf('升交点赤经RAAN = %.2f deg\n', RAAN*180/pi); fprintf('近地点幅角omega = %.2f deg\n', omega*180/pi); fprintf('初平近点角M0 = %.2f deg\n', M0*180/pi); fprintf('轨道周期T = %.2f min\n', T/60); fprintf('轨道面法向量 = [%.2f %.2f %.2f]\n', i_vec(1), i_vec(2), i_vec(3)); fprintf('升交点赤道面法向量 = [%.2f %.2f %.2f]\n', n_vec(1), n_vec(2), n_vec(3)); fprintf('角动量矢量 = [%.2f %.2f %.2f] km^2/s\n', h_vec(1), h_vec(2), h_vec(3)); fprintf('离心率矢量 = [%.2f %.2f %.2f]\n', e_vec(1), e_vec(2), e_vec(3));
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值