GPS从入门到放弃（七） --- GPS卫星位置解算

GPS从入门到放弃（七） — GPS卫星位置解算

上一篇讲了开普勒轨道参数，根据这些参数就可以确定卫星的位置，这一篇我们来实际计算一下。

WGS-84基本参数

首先给出几个WGS-84坐标系中的基本参数：

$a = 6378137[m]$ # 基准椭球体长半径
$f = 1/298.257223563$ # 基准椭球体扁率
$\dot{\Omega}_e = 7.2921151467\times10^{-5}[rad/s]$ # 地球自转角速度
$\mu = 3.986005\times10^{14}[m^3/s^2]$ # 地球引力常数GM
$c = 2.99792458\times10^8[m/s]$ # 真空中的光速

Python代码如下：

# WGS-84基本参数
a = 6378137 # 基准椭球体长半径（m）
f = 1/298.257223563 # 基准椭球体扁率
Omega_e_Dot = 7.2921151467e-5 # 地球自转角速度（rad/s）
mu = 3.986005e14 # 地球引力常数GM（m^3/s^2）
c = 2.99792458e8 # 真空中的光速（m/s）

星历参数

$t_{oe}$ # 星历参考时间
$\sqrt{A}$ # 卫星轨道半长轴A的平方根
$e$ # 卫星轨道偏心率
$i_0$ # $t_{oe}$ 时的轨道倾角
$\Omega_0$ # 周内时为0时的轨道升交点赤经
$\omega$ # 近地点角距
$M_0$ # $t_{oe}$ 时的平近点角
$\Delta_n$ # 卫星平均角速度校正值
$\dot{i}$ # 轨道倾角的变化率
$\dot{\Omega}$ # 轨道升交点赤经的变化率
$C_{uc}$ # 升交点角距余弦调和校正振幅
$C_{us}$ # 升交点角距正弦调和校正振幅
$C_{rc}$ # 轨道半径余弦调和校正振幅
$C_{rs}$ # 轨道半径正弦调和校正振幅
$C_{ic}$ # 轨道倾角余弦调和校正振幅
$C_{is}$ # 轨道倾角正弦调和校正振幅

从网站 ftp://cddis.nasa.gov/gnss/data 下载2019年国庆（10月1日）当天的星历数据，可以得到星历参数的值，Python代码如下：

# 星历参数
t_oe = 2.016000000000E+05
A_sqrt = 5.153681812286E+03
e = 1.475233526435E-02
i_0 = 9.590228562257E-01
Omega_0 = -1.091936976129E-01
omega = 6.837269280624E-01
M_0 = 1.699075304872E+00
Delta_n = 4.599120143243E-09
i_Dot = -3.957307694893E-10
Omega_Dot = -8.244629136182E-09
Cuc = -5.902722477913E-06
Cus = 9.264796972275E-06
Crc = 2.046875000000E+02
Crs = -1.155625000000E+02
Cic = -3.259629011154E-07
Cis = 5.774199962616E-08

下面开始计算。

计算卫星轨道半长轴

A = A_sqrt**2 # 卫星轨道半长轴
print("A={}".format(A))

可得 A=26560436.222287513（米）

计算规化时间

假设我们要计算这个星历发射时刻 $t = 1.993680000000E+05$ 的卫星位置，设规化时间为 $t_k$ ，即为 $t$ 时刻与参考时间 $t_{oe}$ 之间的差异：
$t_k = t - t_{oe}$
要注意 $t$ 值应该在 $t_{oe}$ 前后两小时之间才算有效。
代码如下：

A = A_sqrt**2 # 卫星轨道半长轴
t = 1.993680000000E+05
t_k = t - t_oe
if t_k > 302400:
    t_k -= 604800
if t_k < -302400:
    t_k += 604800
if -7200 <= t_k and t_k <= 7200:
    print("t_k={}".format(t_k))
else:
    print("time t={} is not valid".format(t))

可得 $t_k=-2232.0$ s（秒）

计算校正后的卫星平均角速度

校正后的卫星平均角速度 $n = n_0 + \Delta_n$ ，其中 $n_0$ 为卫星平均角速度，可由 $n_0 = \sqrt{\frac{\mu}{A^3}}$ 求得。代码如下：

import math
n_0 = math.sqrt(mu/A**3) # 卫星平均角速度
n = n_0 + Delta_n # 校正后的卫星平均角速度
print("n={}".format(n))

可得 $n=0.00014585785029794723$ （弧度/秒）

计算近点角

平近点角 $M_k$

$M_k = M_0 + n t_k$ ，其范围在0~2 $\pi$ 之间。

偏近点角 $E$

由开普勒方程，有
$M = E - e \sin E$
可得
$E = M + e \sin E$
为便于计算机求解，利用迭代算法
$E_{j+1} = M + e \sin E_j$
当误差 $|E_{j+1}-E_j|<10^{-8}$ 时停止迭代。

真近点角 $\nu_k$

由
$\cos\nu_k = \frac{\cos E_k - e}{1-e\cos E_k}$
$\sin\nu_k = \frac{\sqrt{1-e^2}\sin E_k}{1-e\cos E_k}$
可得
$\nu_k = \arctan\left(\frac{\sqrt{1-e^2}\sin E_k}{\cos E_k - e}\right)$

代码如下：

# 平近点角
M_k = M_0 + n*t_k
if M_k < 0:
    M_k += 2*math.pi
if M_k > 2*math.pi:
    M_k -= 2*math.pi
print("M_k={}".format(M_k))

# 偏近点角
E_old = M_k
E_new = M_k + e*math.sin(E_old)
i = 1
while abs(E_new - E_old)>1e-8:
    print("i={},E={}".format(i, E_new))
    E_old = E_new
    E_new = M_k + e*math.sin(E_old)
    i += 1
    if (i>10):
        break

E_k = E_new
print("E_k={}".format(E_k))

# 真近点角
cosNu_k = (math.cos(E_k) - e) / (1 - e*math.cos(E_k))
sinNu_k = (math.sqrt(1-e**2)*math.sin(E_k)) / (1 - e*math.cos(E_k))
print("cosNu_k={}".format(cosNu_k))
print("sinNu_k={}".format(sinNu_k))

if cosNu_k == 0:
    if sinNu_k > 0:
        Nu_k = math.pi/2
    else:
        Nu_k = -math.pi/2
else:
    Nu_k = math.atan(sinNu_k/cosNu_k)

if cosNu_k < 0:
    if sinNu_k >= 0:
        Nu_k += math.pi
    else:
        Nu_k -= math.pi

print("Nu_k={}".format(Nu_k))

可得:
平近点角 $M_k=1.3735205830069819$ （弧度），
偏近点角 $E_k=1.3880272058351995$ （弧度），
真近点角 $\nu_k=1.4025538389890888$ （弧度）。

计算升交点角距 $\Phi_k$

$\Phi_k=\nu_k+\omega$

Phi_k = Nu_k + omega
print("Phi_k={}".format(Phi_k))

得到 $\Phi_k=2.086280767051489$ （弧度）

计算摄动校正后的升交点角距 $u_k$ 、卫星矢径长度 $r_k$ 、轨道倾角 $i_k$

根据以下关系计算
$u_k = \Phi_k + \delta u_k$
$r_k = A(1-e\cos E_k)+\delta r_k$
$i_k = i_0 + \dot{i}\cdot t_k + \delta i_k$
其中
$\delta u_k = C_{us}\sin2\Phi_k + C_{uc}\cos2\Phi_k$
$\delta r_k = C_{rs}\sin2\Phi_k + C_{rc}\cos2\Phi_k$
$\delta i_k = C_{is}\sin2\Phi_k + C_{ic}\cos2\Phi_k$
代码如下：

delta_u_k = Cus*math.sin(2*Phi_k) + Cuc*math.cos(2*Phi_k)
delta_r_k = Crs*math.sin(2*Phi_k) + Crc*math.cos(2*Phi_k)
delta_i_k = Cis*math.sin(2*Phi_k) + Cic*math.cos(2*Phi_k)
print("delta_u_k={}".format(delta_u_k))
print("delta_r_k={}".format(delta_r_k))
print("delta_i_k={}".format(delta_i_k))

u_k = Phi_k + delta_u_k
r_k = A*(1-e*math.cos(E_k)) + delta_r_k
i_k = i_0 + i_Dot*t_k + delta_i_k
print("u_k={}".format(u_k))
print("r_k={}".format(r_k))
print("i_k={}".format(i_k))

可得校正后的:
升交点角距 $u_k=2.086275853661298$ （弧度），
卫星矢径长度 $r_k=26489214.0423851$ （米），
轨道倾角 $i_k=0.9590238575068554$ （弧度）。

坐标转换

由升交点角距和卫星矢径长度即可确定卫星在轨道平面的位置，当然，是用极坐标表示的。我们将其转换为直角坐标系：
$x'_k = r_k\cos u_k \\ y'_k = r_k\sin u_k \\ z'_k = 0$
然后计算升交点赤经 $\Omega_k$
$\Omega_k = \Omega_0 + (\dot{\Omega} - \dot{\Omega}_e) t_k - \dot{\Omega}_e t_{oe}$
最后将卫星在轨道直角坐标系中的坐标 $(x'_k, y'_k, z'_k)$ 转换为在WGS-84坐标系中的坐标 $(x_k, y_k, z_k)$
$x_k = x'_k\cos\Omega_k - y'_k\cos i_k \sin \Omega_k \\ y_k = x'_k\sin\Omega_k + x'_k\cos i_k \cos \Omega_k \\ z_k = y'_k\sin i_k$
代码如下：

x_p_k = r_k * math.cos(u_k)
y_p_k = r_k * math.sin(u_k)
print("x_p_k={}".format(x_p_k))
print("y_p_k={}".format(y_p_k))

Omega_k = Omega_0 + (Omega_Dot - Omega_e_Dot)*t_k - Omega_e_Dot*t_oe
print("Omega_k={}".format(Omega_k))

x_k = x_p_k*math.cos(Omega_k) - y_p_k*math.cos(i_k)*math.sin(Omega_k)
y_k = x_p_k*math.sin(Omega_k) + y_p_k*math.cos(i_k)*math.cos(Omega_k)
z_k = y_p_k*math.sin(i_k)
print("x_k={}".format(x_k))
print("y_k={}".format(y_k))
print("z_k={}".format(z_k))

最后得到卫星在WGS-84坐标系中的位置坐标为：
$\left[ \begin{array}{ll} x_k \\ y_k \\ z_k \end{array} \right]= \left[ \begin{array}{ll} 17927326.1391382 \\ 4931779.063749035 \\ 18867087.569379408 \end{array} \right](米)$

tyst08

发布了63 篇原创文章 · 获赞 16 · 访问量 3万+

私信关注