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

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

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

WGS-84基本参数

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

• a = 6378137 [ m ] a = 6378137[m] a=6378137[m] # 基准椭球体长半径
• f = 1 / 298.257223563 f = 1/298.257223563 f=1/298.257223563 # 基准椭球体扁率
• Ω ˙ e = 7.2921151467 × 1 0 − 5 [ r a d / s ] \dot{\Omega}_e = 7.2921151467\times10^{-5}[rad/s] Ω˙e​=7.2921151467×10−5[rad/s] # 地球自转角速度
• μ = 3.986005 × 1 0 14 [ m 3 / s 2 ] \mu = 3.986005\times10^{14}[m^3/s^2] μ=3.986005×1014[m3/s2] # 地球引力常数GM
• c = 2.99792458 × 1 0 8 [ m / s ] c = 2.99792458\times10^8[m/s] c=2.99792458×108[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 o e t_{oe} toe​ # 星历参考时间
• A \sqrt{A} A ​ # 卫星轨道半长轴A的平方根
• e e e # 卫星轨道偏心率
• i 0 i_0 i0​ # t o e t_{oe} toe​时的轨道倾角
• Ω 0 \Omega_0 Ω0​ # 周内时为0时的轨道升交点赤经
• ω \omega ω # 近地点角距
• M 0 M_0 M0​ # t o e t_{oe} toe​时的平近点角
• Δ n \Delta_n Δn​ # 卫星平均角速度校正值
• i ˙ \dot{i} i˙ # 轨道倾角的变化率
• Ω ˙ \dot{\Omega} Ω˙ # 轨道升交点赤经的变化率
• C u c C_{uc} Cuc​ # 升交点角距余弦调和校正振幅
• C u s C_{us} Cus​ # 升交点角距正弦调和校正振幅
• C r c C_{rc} Crc​ # 轨道半径余弦调和校正振幅
• C r s C_{rs} Crs​ # 轨道半径正弦调和校正振幅
• C i c C_{ic} Cic​ # 轨道倾角余弦调和校正振幅
• C i s C_{is} Cis​ # 轨道倾角正弦调和校正振幅

从网站 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.993680000000 E + 05 t = 1.993680000000E+05 t=1.993680000000E+05 的卫星位置，设规化时间为 t k t_k tk​，即为 t t t 时刻与参考时间 t o e t_{oe} toe​ 之间的差异：
t k = t − t o e t_k = t &＃8211; t_{oe} tk​=t−toe​
要注意 t t t 值应该在 t o e t_{oe} toe​ 前后两小时之间才算有效。
代码如下：

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 t_k=-2232.0 tk​=−2232.0s（秒）

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

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

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


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

计算近点角

1. 平近点角 M k M_k Mk​

M k = M 0 + n t k M_k = M_0 + n t_k Mk​=M0​+ntk​，其范围在0~2 π \pi π 之间。

1. 偏近点角 E E E

由开普勒方程，有
M = E − e sin ⁡ E M = E &＃8211; e \sin E M=E−esinE
可得
E = M + e sin ⁡ E E = M + e \sin E E=M+esinE
为便于计算机求解，利用迭代算法
E j + 1 = M + e sin ⁡ E j E_{j+1} = M + e \sin E_j Ej+1​=M+esinEj​
当误差 ∣ E j + 1 − E j ∣ <1 0 − 8 |E_{j+1}-E_j|<10^{-8} ∣Ej+1​−Ej​∣<10−8 时停止迭代。

1. 真近点角 ν k \nu_k νk​

由
cos ⁡ ν k = cos ⁡ E k − e 1 − e cos ⁡ E k \cos\nu_k = \frac{\cos E_k &＃8211; e}{1-e\cos E_k} cosνk​=1−ecosEk​cosEk​−e​
sin ⁡ ν k = 1 − e 2 sin ⁡ E k 1 − e cos ⁡ E k \sin\nu_k = \frac{\sqrt{1-e^2}\sin E_k}{1-e\cos E_k} sinνk​=1−ecosEk​1−e2 ​sinEk​​
可得
ν k = arctan ⁡ ( 1 − e 2 sin ⁡ E k cos ⁡ E k − e ) \nu_k = \arctan\left(\frac{\sqrt{1-e^2}\sin E_k}{\cos E_k &＃8211; e}\right) νk​=arctan(cosEk​−e1−e2 ​sinEk​​)

代码如下：

# 平近点角
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 M_k=1.3735205830069819 Mk​=1.3735205830069819（弧度），
偏近点角 E k = 1.3880272058351995 E_k=1.3880272058351995 Ek​=1.3880272058351995（弧度），
真近点角 ν k = 1.4025538389890888 \nu_k=1.4025538389890888 νk​=1.4025538389890888（弧度）。

计算升交点角距 Φ k \Phi_k Φk​

Φ k = ν k + ω \Phi_k=\nu_k+\omega Φk​=νk​+ω

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


得到 Φ k = 2.086280767051489 \Phi_k=2.086280767051489 Φk​=2.086280767051489（弧度）

计算摄动校正后的升交点角距 u k u_k uk​、卫星矢径长度 r k r_k rk​、轨道倾角 i k i_k ik​

根据以下关系计算
u k = Φ k + δ u k u_k = \Phi_k + \delta u_k uk​=Φk​+δuk​
r k = A ( 1 − e cos ⁡ E k ) + δ r k r_k = A(1-e\cos E_k)+\delta r_k rk​=A(1−ecosEk​)+δrk​
i k = i 0 + i ˙ ⋅ t k + δ i k i_k = i_0 + \dot{i}\cdot t_k + \delta i_k ik​=i0​+i˙⋅tk​+δik​
其中
δ u k = C u s sin ⁡ 2 Φ k + C u c cos ⁡ 2 Φ k \delta u_k = C_{us}\sin2\Phi_k + C_{uc}\cos2\Phi_k δuk​=Cus​sin2Φk​+Cuc​cos2Φk​
δ r k = C r s sin ⁡ 2 Φ k + C r c cos ⁡ 2 Φ k \delta r_k = C_{rs}\sin2\Phi_k + C_{rc}\cos2\Phi_k δrk​=Crs​sin2Φk​+Crc​cos2Φk​
δ i k = C i s sin ⁡ 2 Φ k + C i c cos ⁡ 2 Φ k \delta i_k = C_{is}\sin2\Phi_k + C_{ic}\cos2\Phi_k δik​=Cis​sin2Φk​+Cic​cos2Φ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 u_k=2.086275853661298 uk​=2.086275853661298（弧度），
卫星矢径长度 r k = 26489214.0423851 r_k=26489214.0423851 rk​=26489214.0423851（米），
轨道倾角 i k = 0.9590238575068554 i_k=0.9590238575068554 ik​=0.9590238575068554（弧度）。

坐标转换

由升交点角距和卫星矢径长度即可确定卫星在轨道平面的位置，当然，是用极坐标表示的。我们将其转换为直角坐标系：
x k ′ = r k cos ⁡ u k y k ′ = r k sin ⁡ u k z k ′ = 0 x&＃8217;_k = r_k\cos u_k \\ y&＃8217;_k = r_k\sin u_k \\ z&＃8217;_k = 0 xk′​=rk​cosuk​yk′​=rk​sinuk​zk′​=0
然后计算升交点赤经 Ω k \Omega_k Ωk​
Ω k = Ω 0 + ( Ω ˙ − Ω ˙ e ) t k − Ω ˙ e t o e \Omega_k = \Omega_0 + (\dot{\Omega} &＃8211; \dot{\Omega}_e) t_k &＃8211; \dot{\Omega}_e t_{oe} Ωk​=Ω0​+(Ω˙−Ω˙e​)tk​−Ω˙e​toe​
最后将卫星在轨道直角坐标系中的坐标 ( x k ′ , y k ′ , z k ′ ) (x&＃8217;_k, y&＃8217;_k, z&＃8217;_k) (xk′​,yk′​,zk′​) 转换为在WGS-84坐标系中的坐标 ( x k , y k , z k ) (x_k, y_k, z_k) (xk​,yk​,zk​)
x k = x k ′ cos ⁡ Ω k − y k ′ cos ⁡ i k sin ⁡ Ω k y k = x k ′ sin ⁡ Ω k + y k ′ cos ⁡ i k cos ⁡ Ω k z k = y k ′ sin ⁡ i k x_k = x&＃8217;_k\cos\Omega_k &＃8211; y&＃8217;_k\cos i_k \sin \Omega_k \\ y_k = x&＃8217;_k\sin\Omega_k + y&＃8217;_k\cos i_k \cos \Omega_k \\ z_k = y&＃8217;_k\sin i_k xk​=xk′​cosΩk​−yk′​cosik​sinΩk​yk​=xk′​sinΩk​+yk′​cosik​cosΩk​zk​=yk′​sinik​
代码如下：

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坐标系中的位置坐标为：
[ x k y k z k ] = [ 17927326.1391382 4931779.063749035 18867087.569379408 ] ( 米 ) \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](米) ⎣⎡​xk​yk​zk​​⎦⎤​=⎣⎡​17927326.13913824931779.06374903518867087.569379408​⎦⎤​(米)