2024MathModeling/Problem1.py at master · hengxt/2024MathModeling · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
import matplotlib
import matplotlib.pyplot as plt
import numpy as np

matplotlib.use('TkAgg')

# 常量
mu = 398600.4418  # 地球引力常数, km^3/s^2


## 地心天球参考系（Geocentric Celestial Reference System，GCRS）
## 画一个地心天球参考系的三维空间图，并通过Ω, i, ω可以实现卫星位置的变化

class GCRSDrawer:

    def __init__(self, ecc, alt, Omega, inc, omega, theta):
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(111, projection='3d')
        self.ax.set_xlabel('X axis')
        self.ax.set_ylabel('Y axis')
        self.ax.set_zlabel('Z axis')
        self.plot_earth()
        self.plot_satellite(ecc, alt, Omega, inc, omega, theta)

    def plot_earth(self):
        u, v = np.mgrid[0:2 * np.pi:50j, 0:np.pi:25j]
        # 地球半径
        r = 6378.137
        x = np.cos(u) * np.sin(v) * r
        y = np.sin(u) * np.sin(v) * r
        z = np.cos(v) * r
        self.ax.plot_surface(x, y, z, color="b", alpha=0.2)

    def plot_satellite(self, ecc, alt, Omega, inc, omega, theta):
        _r_GCRS, _v_GCRS = self.position_GCRS(ecc, alt, Omega, inc, omega, theta)

        # 绘制卫星位置
        self.ax.scatter(_r_GCRS[0], _r_GCRS[1], _r_GCRS[2], color="r", label='Satellite Position')
        # 画出
        # self.ax.

        # 如果需要显示速度矢量
        self.ax.quiver(_r_GCRS[0], _r_GCRS[1], _r_GCRS[2],
                       _v_GCRS[0], _v_GCRS[1], _v_GCRS[2],
                       length=1000, normalize=True, color='g', label='Velocity Vector')

        # 添加图例
        self.ax.legend()

        # 显示图形
        plt.show()

    @staticmethod
    def position_GCRS(ecc, alt, Omega, inc, omega, theta):
        # 计算旋转矩阵
        def rotation_matrix(_Omega, _inc, _omega):
            R3_Omega = np.array([[np.cos(-_Omega), np.sin(-_Omega), 0],
                                 [-np.sin(-_Omega), np.cos(-_Omega), 0],
                                 [0, 0, 1]])

            R1_inc = np.array([[1, 0, 0],
                               [0, np.cos(-_inc), np.sin(-_inc)],
                               [0, -np.sin(-_inc), np.cos(-_inc)]])

            R3_omega = np.array([[np.cos(-_omega), np.sin(-_omega), 0],
                                 [-np.sin(-_omega), np.cos(-_omega), 0],
                                 [0, 0, 1]])

            return R3_Omega @ R1_inc @ R3_omega

        # 计算近地点距 r 和速度
        r = alt ** 2 / (mu * (1 + ecc * np.cos(theta)))  ## km
        v_r = (mu / alt) * ecc * np.sin(theta)  ## km/s
        v_theta = alt / r  ## km/s
        # 在轨道平面的二维位置和速度矢量
        r_orb = np.array([r * np.cos(theta), r * np.sin(theta), 0])
        v_orb = np.array([v_r * np.cos(theta) - v_theta * np.sin(theta),
                          v_r * np.sin(theta) + v_theta * np.cos(theta), 0])
        # 计算最终位置和速度矢量
        R = rotation_matrix(Omega, inc, omega)
        _r_GCRS = R @ r_orb
        _t_GCRS = R @ v_orb
        return _r_GCRS, _t_GCRS


if __name__ == '__main__':
    # 给定的轨道根数
    e = 2.06136076e-3
    h = 5.23308462e4  # km^2/s
    Ω = 5.69987423  # rad
    i = 1.69931232  # rad
    ω = 4.10858621  # rad
    θ = 3.43807372  # rad
    r_GCRS, v_GCRS = GCRSDrawer.position_GCRS(e, h, Ω, i, ω, θ)
    # 输出结果
    print(f"卫星在GCRS中的位置 (X, Y, Z): {r_GCRS}")
    print(f"卫星在GCRS中的速度 (v_x, v_y, v_z): {v_GCRS}")
    drawer = GCRSDrawer(e, h, Ω, i, ω, θ)
    plt.show()