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

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

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

WGS-84基本参数

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

• $a=6378137[m]$ # 基准椭球体长半径
• $f=1/298.257223563$ # 基准椭球体扁率
• ${\dot{\Omega }}_e=7.2921151467\times 10^{-5}[rad/s]$ # 地球自转角速度
• $\mu =3.986005\times 10^{14}[m^3/s^2]$ # 地球引力常数GM
• $c=2.99792458\times 10^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$（弧度/秒）

计算近点角

1. 平近点角$M_k$

$M_k=M_0+n{t}_k$，其范围在0~2 $\pi$ 之间。

2. 偏近点角 $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}$ 时停止迭代。

3. 真近点角 ${\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}\sin 2{\Phi }_k+C_{uc}\cos 2{\Phi }_k$$
$$\delta r_k=C_{rs}\sin 2{\Phi }_k+C_{rc}\cos 2{\Phi }_k$$
$$\delta i_k=C_{is}\sin 2{\Phi }_k+C_{ic}\cos 2{\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$（弧度）。

坐标转换

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

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}{l}{x}_k\\ y_k\\ z_k\end{array}\right]=\left[\begin{array}{l}17927326.1391382\\ 4931779.063749035\\ 18867087.569379408\end{array}\right](米)$$