让GNSS&RTK不再难【第二天-第1部分】

发布于:2024-06-10 ⋅ 阅读:(274) ⋅ 点赞:(0)

第9讲 单点定位技术理论

9.1 伪距观测值方程

伪距观测值公式表示为:

P r s = ρ r s − c δ t s + c δ t r + I r s + T r s + e P P_r^s = \rho_r^s - c\delta t^s + c\delta t_r + I_r^s + T_r^s + e_P Prs​=ρrs​−cδts+cδtr​+Irs​+Trs​+eP​

其中:

  • P r s P_r^s Prs​ 为卫星 s s s 到接收机 r r r 的伪距观测值
  • ρ r s \rho_r^s ρrs​ 为卫星 s s s 到接收机 r r r 的真实距离
  • c c c 为光速
  • δ t s \delta t^s δts 为卫星钟差
  • δ t r \delta t_r δtr​ 为接收机钟差
  • I r s I_r^s Irs​ 为电离层延迟
  • T r s T_r^s Trs​ 为对流层延迟
  • e P e_P eP​ 为伪距观测值噪声,单位为米

假设某GNSS接收机接收到来自某卫星的伪距观测值 P r s = 20200000 P_r^s = 20200000 Prs​=20200000 米,已知:

  • ρ r s = 20185000 \rho_r^s = 20185000 ρrs​=20185000 米
  • δ t s − δ t r = 50 \delta t^s - \delta t_r = 50 δts−δtr​=50 纳秒(1 纳秒 = 1 0 − 9 10^{-9} 10−9 秒)
  • I r s = 3 I_r^s = 3 Irs​=3 米
  • T r s = 2 T_r^s = 2 Trs​=2 米

将这些值代入公式:

P r s = ρ r s − c δ t s + c δ t r + I r s + T r s + e P P_r^s = \rho_r^s - c\delta t^s + c\delta t_r + I_r^s + T_r^s + e_P Prs​=ρrs​−cδts+cδtr​+Irs​+Trs​+eP​

其中 c = 299792458 c = 299792458 c=299792458 米/秒,所以:

P r s = 20185000 − 299792458 × 50 × 1 0 − 9 + 3 + 2 + e P P_r^s = 20185000 - 299792458 \times 50 \times 10^{-9} + 3 + 2 + e_P Prs​=20185000−299792458×50×10−9+3+2+eP​

P r s = 20185000 − 14.989623 + 3 + 2 + e P P_r^s = 20185000 - 14.989623 + 3 + 2 + e_P Prs​=20185000−14.989623+3+2+eP​

假设噪声 e P e_P eP​ 很小,可以忽略不计:

P r s ≈ 20185000 − 14.99 + 3 + 2 = 20184990.01 米 P_r^s \approx 20185000 - 14.99 + 3 + 2 = 20184990.01 \text{米} Prs​≈20185000−14.99+3+2=20184990.01米

因此,计算得到的伪距观测值约为20184990.01米,接近接收到的20200000米。

通过以上例子,我们可以看到如何利用伪距观测值方程计算接收机和卫星之间的距离,并考虑各种误差因素,如钟差、电离层延迟和对流层延迟。

9.2 定位方程

对于伪距观测值:

P i s = ( X s − X r ) 2 + ( Y s − Y r ) 2 + ( Z s − Z r ) 2 P_i^s = \sqrt{(X^s - X_r)^2 + (Y^s - Y_r)^2 + (Z^s - Z_r)^2} Pis​=(Xs−Xr​)2+(Ys−Yr​)2+(Zs−Zr​)2 ​

如果我们令卫星位置向量 P s = ( X s , Y s , Z s ) P^s = (X^s, Y^s, Z^s) Ps=(Xs,Ys,Zs) 接收机位置向量 P r = ( X r , Y r , Z r ) P_r = (X_r, Y_r, Z_r) Pr​=(Xr​,Yr​,Zr​) 向量差 Δ P = P s − P r \Delta P = P^s - P_r ΔP=Ps−Pr​ 向量 Δ P \Delta P ΔP 方向上的单位向量,即为 u \mathbf{u} u。那么有:

ρ i s = Δ P ⋅ u \rho_i^s = \Delta P \cdot \mathbf{u} ρis​=ΔP⋅u

其中 u \mathbf{u} u 为 Δ P \Delta P ΔP 的单位向量。

如果我们已知接收机的概略位置为 ( X 0 , Y 0 , Z 0 ) (X_0, Y_0, Z_0) (X0​,Y0​,Z0​),有:

X r = X 0 + d x X_r = X_0 + dx Xr​=X0​+dx
Y r = Y 0 + d y Y_r = Y_0 + dy Yr​=Y0​+dy
Z r = Z 0 + d z Z_r = Z_0 + dz Zr​=Z0​+dz

即有:

P r = P r 0 + Δ P P_r = P_{r_0} + \Delta P Pr​=Pr0​​+ΔP
Δ P = P s − P r 0 − Δ P \Delta P = P^s - P_{r_0} - \Delta P ΔP=Ps−Pr0​​−ΔP

其中 P r 0 P_{r_0} Pr0​​ 为接收机概略位置向量, Δ P \Delta P ΔP 为概略位置偏差

ρ i s = ( P s − P r 0 − Δ P ) ⋅ u \rho_i^s = (P^s - P_{r_0} - \Delta P) \cdot \mathbf{u} ρis​=(Ps−Pr0​​−ΔP)⋅u

同时我们计算卫星到接收机概略位置的距离,理论上来说如果概略位置偏差不太大,那么单位向量的差异就不大,甚至可以忽略。

u 0 = ( X s − X 0 R 0 s , Y s − Y 0 R 0 s , Z s − Z 0 R 0 s ) \mathbf{u}_0 = \left( \frac{X^s - X_0}{R_0^s}, \frac{Y^s - Y_0}{R_0^s}, \frac{Z^s - Z_0}{R_0^s} \right) u0​=(R0s​Xs−X0​​,R0s​Ys−Y0​​,R0s​Zs−Z0​​)

ρ i s = ( P s − P r 0 − Δ P ) ⋅ u 0 = ( P s − P r 0 ) ⋅ u 0 − Δ P ⋅ u 0 \rho_i^s = (P^s - P_{r_0} - \Delta P) \cdot \mathbf{u}_0 = (P^s - P_{r_0}) \cdot \mathbf{u}_0 - \Delta P \cdot \mathbf{u}_0 ρis​=(Ps−Pr0​​−ΔP)⋅u0​=(Ps−Pr0​​)⋅u0​−ΔP⋅u0​

进一步简化后:

ρ i s = R 0 s − X s − X 0 R 0 s d x − Y s − Y 0 R 0 s d y − Z s − Z 0 R 0 s d z \rho_i^s = R_0^s - \frac{X^s - X_0}{R_0^s}dx - \frac{Y^s - Y_0}{R_0^s}dy - \frac{Z^s - Z_0}{R_0^s}dz ρis​=R0s​−R0s​Xs−X0​​dx−R0s​Ys−Y0​​dy−R0s​Zs−Z0​​dz

其中:

R 0 s = ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 R_0^s = \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} R0s​=(Xs−X0​)2+(Ys−Y0​)2+(Zs−Z0​)2 ​

这样就将待估参数由接收机位置,变成了相对于接收机概略位置的偏量 (dx, dy, dz)。

进一步整理以下公式:

P i s = ( X s − X 0 + d x ) 2 + ( Y s − Y 0 + d y ) 2 + ( Z s − Z 0 + d z ) 2 − c δ t + I i s − T i s − ϵ P i s P_i^s = \sqrt{(X^s - X_0 + dx)^2 + (Y^s - Y_0 + dy)^2 + (Z^s - Z_0 + dz)^2} - c\delta t + I_i^s - T_i^s - \epsilon_{P_i^s} Pis​=(Xs−X0​+dx)2+(Ys−Y0​+dy)2+(Zs−Z0​+dz)2 ​−cδt+Iis​−Tis​−ϵPis​​

如果我们使用电离层和对流层模型对大气误差进行估计,并使用广播星历计算卫星位置和卫星钟改正,并在公式中扣除:

P i s = ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 + c δ t − I i s − T i s − ϵ P i s − X s − X 0 R 0 s d x − Y s − Y 0 R 0 s d y − Z s − Z 0 R 0 s d z P_i^s = \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} + c\delta t - I_i^s - T_i^s - \epsilon_{P_i^s} - \frac{X^s - X_0}{R_0^s}dx - \frac{Y^s - Y_0}{R_0^s}dy - \frac{Z^s - Z_0}{R_0^s}dz Pis​=(Xs−X0​)2+(Ys−Y0​)2+(Zs−Z0​)2 ​+cδt−Iis​−Tis​−ϵPis​​−R0s​Xs−X0​​dx−R0s​Ys−Y0​​dy−R0s​Zs−Z0​​dz

那么整理可得:

P i s = l i s − m i s d x − n i s d y − p i s d z + c δ t P_i^s = l_i^s - m_i^s dx - n_i^s dy - p_i^s dz + c\delta t Pis​=lis​−mis​dx−nis​dy−pis​dz+cδt

其中:

l i s = P i s − ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 + c δ t − I i s − T i s − ϵ P i s l_i^s = P_i^s - \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} + c\delta t - I_i^s - T_i^s - \epsilon_{P_i^s} lis​=Pis​−(Xs−X0​)2+(Ys−Y0​)2+(Zs−Z0​)2 ​+cδt−Iis​−Tis​−ϵPis​​ 称为先验残差。

m i s = X s − X 0 R 0 s , n i s = Y s − Y 0 R 0 s , p i s = Z s − Z 0 R 0 s m_i^s = \frac{X^s - X_0}{R_0^s}, n_i^s = \frac{Y^s - Y_0}{R_0^s}, p_i^s = \frac{Z^s - Z_0}{R_0^s} mis​=R0s​Xs−X0​​,nis​=R0s​Ys−Y0​​,pis​=R0s​Zs−Z0​​ 为从概略位置到卫星的方向余弦。

如果可以观测到同一系统的四个卫星,就可以估计出四个未知状态量。

{ l 1 s = l 1 s − m 1 s d x − n 1 s d y − p 1 s d z + c δ t l 2 s = l 2 s − m 2 s d x − n 2 s d y − p 2 s d z + c δ t l 3 s = l 3 s − m 3 s d x − n 3 s d y − p 3 s d z + c δ t l 4 s = l 4 s − m 4 s d x − n 4 s d y − p 4 s d z + c δ t \begin{cases} l_1^s = l_1^s - m_1^s dx - n_1^s dy - p_1^s dz + c\delta t \\ l_2^s = l_2^s - m_2^s dx - n_2^s dy - p_2^s dz + c\delta t \\ l_3^s = l_3^s - m_3^s dx - n_3^s dy - p_3^s dz + c\delta t \\ l_4^s = l_4^s - m_4^s dx - n_4^s dy - p_4^s dz + c\delta t \end{cases} ⎩ ⎨ ⎧​l1s​=l1s​−m1s​dx−n1s​dy−p1s​dz+cδtl2s​=l2s​−m2s​dx−n2s​dy−p2s​dz+cδtl3s​=l3s​−m3s​dx−n3s​dy−p3s​dz+cδtl4s​=l4s​−m4s​dx−n4s​dy−p4s​dz+cδt​

整理成矩阵形式,并用残差值表示:

V = [ l 1 s l 2 s l 3 s l 4 s ] = [ m 1 s n 1 s p 1 s − 1 m 2 s n 2 s p 2 s − 1 m 3 s n 3 s p 3 s − 1 m 4 s n 4 s p 4 s − 1 ] ⋅ δ z = A ⋅ δ z V = \begin{bmatrix} l_1^s \\ l_2^s \\ l_3^s \\ l_4^s \end{bmatrix} = \begin{bmatrix} m_1^s & n_1^s & p_1^s & -1 \\ m_2^s & n_2^s & p_2^s & -1 \\ m_3^s & n_3^s & p_3^s & -1 \\ m_4^s & n_4^s & p_4^s & -1 \end{bmatrix} \cdot \delta z = A \cdot \delta z V= ​l1s​l2s​l3s​l4s​​ ​= ​m1s​m2s​m3s​m4s​​n1s​n2s​n3s​n4s​​p1s​p2s​p3s​p4s​​−1−1−1−1​ ​⋅δz=A⋅δz

即有以上公式:

V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz

我们将 V V V 称为先验残差矩阵, A A A 为设计矩阵或称为jacobian矩阵, δ z \delta z δz 为待估状态量。

以上只是单独一个系统,比如仅仅为GPS定位,那如果我增加北斗系统,那方程该文如何表示。

实例

假设我们有以下四个卫星的伪距观测值 P 1 , P 2 , P 3 , P 4 P_1, P_2, P_3, P_4 P1​,P2​,P3​,P4​,以及它们的坐标 ( X 1 , Y 1 , Z 1 ) , ( X 2 , Y 2 , Z 2 ) , ( X 3 , Y 3 , Z 3 ) , ( X 4 , Y 4 , Z 4 ) (X_1, Y_1, Z_1), (X_2, Y_2, Z_2), (X_3, Y_3, Z_3), (X_4, Y_4, Z_4) (X1​,Y1​,Z1​),(X2​,Y2​,Z2​),(X3​,Y3​,Z3​),(X4​,Y4​,Z4​) 和接收机的概略位置 ( X 0 , Y 0 , Z 0 ) (X_0, Y_0, Z_0) (X0​,Y0​,Z0​)。

  1. 计算每个卫星到接收机概略位置的距离:

    计算从接收机的概略位置到每个卫星的距离,公式如下:

    R 0 1 = ( X 1 − X 0 ) 2 + ( Y 1 − Y 0 ) 2 + ( Z 1 − Z 0 ) 2 R_0^1 = \sqrt{(X_1 - X_0)^2 + (Y_1 - Y_0)^2 + (Z_1 - Z_0)^2} R01​=(X1​−X0​)2+(Y1​−Y0​)2+(Z1​−Z0​)2 ​

    R 0 2 = ( X 2 − X 0 ) 2 + ( Y 2 − Y 0 ) 2 + ( Z 2 − Z 0 ) 2 R_0^2 = \sqrt{(X_2 - X_0)^2 + (Y_2 - Y_0)^2 + (Z_2 - Z_0)^2} R02​=(X2​−X0​)2+(Y2​−Y0​)2+(Z2​−Z0​)2 ​

    R 0 3 = ( X 3 − X 0 ) 2 + ( Y 3 − Y 0 ) 2 + ( Z 3 − Z 0 ) 2 R_0^3 = \sqrt{(X_3 - X_0)^2 + (Y_3 - Y_0)^2 + (Z_3 - Z_0)^2} R03​=(X3​−X0​)2+(Y3​−Y0​)2+(Z3​−Z0​)2 ​

    R 0 4 = ( X 4 − X 0 ) 2 + ( Y 4 − Y 0 ) 2 + ( Z 4 − Z 0 ) 2 R_0^4 = \sqrt{(X_4 - X_0)^2 + (Y_4 - Y_0)^2 + (Z_4 - Z_0)^2} R04​=(X4​−X0​)2+(Y4​−Y0​)2+(Z4​−Z0​)2 ​

    这些距离表示从概略位置到每个卫星的几何距离。

  2. 计算方向余弦:

    方向余弦用于表示从接收机概略位置到每个卫星的方向向量。计算如下:

    m 1 = X 1 − X 0 R 0 1 , n 1 = Y 1 − Y 0 R 0 1 , p 1 = Z 1 − Z 0 R 0 1 m_1 = \frac{X_1 - X_0}{R_0^1}, \quad n_1 = \frac{Y_1 - Y_0}{R_0^1}, \quad p_1 = \frac{Z_1 - Z_0}{R_0^1} m1​=R01​X1​−X0​​,n1​=R01​Y1​−Y0​​,p1​=R01​Z1​−Z0​​

    m 2 = X 2 − X 0 R 0 2 , n 2 = Y 2 − Y 0 R 0 2 , p 2 = Z 2 − Z 0 R 0 2 m_2 = \frac{X_2 - X_0}{R_0^2}, \quad n_2 = \frac{Y_2 - Y_0}{R_0^2}, \quad p_2 = \frac{Z_2 - Z_0}{R_0^2} m2​=R02​X2​−X0​​,n2​=R02​Y2​−Y0​​,p2​=R02​Z2​−Z0​​

    m 3 = X 3 − X 0 R 0 3 , n 3 = Y 3 − Y 0 R 0 3 , p 3 = Z 3 − Z 0 R 0 3 m_3 = \frac{X_3 - X_0}{R_0^3}, \quad n_3 = \frac{Y_3 - Y_0}{R_0^3}, \quad p_3 = \frac{Z_3 - Z_0}{R_0^3} m3​=R03​X3​−X0​​,n3​=R03​Y3​−Y0​​,p3​=R03​Z3​−Z0​​

    m 4 = X 4 − X 0 R 0 4 , n 4 = Y 4 − Y 0 R 0 4 , p 4 = Z 4 − Z 0 R 0 4 m_4 = \frac{X_4 - X_0}{R_0^4}, \quad n_4 = \frac{Y_4 - Y_0}{R_0^4}, \quad p_4 = \frac{Z_4 - Z_0}{R_0^4} m4​=R04​X4​−X0​​,n4​=R04​Y4​−Y0​​,p4​=R04​Z4​−Z0​​

    这些方向余弦描述了从接收机概略位置到每个卫星的单位方向向量。

  3. 形成设计矩阵 A A A 和先验残差矩阵 V V V:

    设计矩阵 A A A 和先验残差矩阵 V V V 用于表示伪距观测方程的线性形式:

    A = [ m 1 n 1 p 1 − 1 m 2 n 2 p 2 − 1 m 3 n 3 p 3 − 1 m 4 n 4 p 4 − 1 ] A = \begin{bmatrix} m_1 & n_1 & p_1 & -1 \\ m_2 & n_2 & p_2 & -1 \\ m_3 & n_3 & p_3 & -1 \\ m_4 & n_4 & p_4 & -1 \end{bmatrix} A= ​m1​m2​m3​m4​​n1​n2​n3​n4​​p1​p2​p3​p4​​−1−1−1−1​ ​

    先验残差矩阵 $ V $ 计算如下:

    V = [ l 1 l 2 l 3 l 4 ] = [ P 1 − R 0 1 + c δ t − I 1 − T 1 − ϵ P 1 P 2 − R 0 2 + c δ t − I 2 − T 2 − ϵ P 2 P 3 − R 0 3 + c δ t − I 3 − T 3 − ϵ P 3 P 4 − R 0 4 + c δ t − I 4 − T 4 − ϵ P 4 ] V = \begin{bmatrix} l_1 \\ l_2 \\ l_3 \\ l_4 \end{bmatrix} = \begin{bmatrix} P_1 - R_0^1 + c\delta t - I_1 - T_1 - \epsilon_{P_1} \\ P_2 - R_0^2 + c\delta t - I_2 - T_2 - \epsilon_{P_2} \\ P_3 - R_0^3 + c\delta t - I_3 - T_3 - \epsilon_{P_3} \\ P_4 - R_0^4 + c\delta t - I_4 - T_4 - \epsilon_{P_4} \end{bmatrix} V= ​l1​l2​l3​l4​​ ​= ​P1​−R01​+cδt−I1​−T1​−ϵP1​​P2​−R02​+cδt−I2​−T2​−ϵP2​​P3​−R03​+cδt−I3​−T3​−ϵP3​​P4​−R04​+cδt−I4​−T4​−ϵP4​​​ ​

    这里的 l i l_i li​ 表示每个伪距观测值减去计算得到的几何距离并进行误差修正后的值。

  4. 解方程 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz 以估计状态量 δ z = [ d x d y d z c δ t ] T \delta z = \begin{bmatrix} dx & dy & dz & c\delta t \end{bmatrix}^T δz=[dx​dy​dz​cδt​]T:

    通过求解线性方程组 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz,我们可以估计状态量 δ z \delta z δz,即接收机位置的修正量和接收机钟差:

    δ z = ( A T A ) − 1 A T V \delta z = (A^T A)^{-1} A^T V δz=(ATA)−1ATV

    这里, ( A T A ) − 1 A T (A^T A)^{-1} A^T (ATA)−1AT 是伪逆矩阵,用于求解最小二乘问题。

  5. 更新接收机位置:

    根据估计的修正量,更新接收机的位置:

    X r = X 0 + d x , Y r = Y 0 + d y , Z r = Z 0 + d z X_r = X_0 + dx, \quad Y_r = Y_0 + dy, \quad Z_r = Z_0 + dz Xr​=X0​+dx,Yr​=Y0​+dy,Zr​=Z0​+dz

    最终,我们得到了更精确的接收机位置 ( X r , Y r , Z r ) (X_r, Y_r, Z_r) (Xr​,Yr​,Zr​)。

通过以上步骤,可以得到接收机的精确位置偏差,并更新接收机的概略位置,从而得到更精确的定位结果。

9.3 多系统定位方程

由于GPS和BDS是两个系统,所以主要表现在接收机端的误差会存在差异,这些误差会被接收机钟进行吸收,所以我们一般可以将GPS和BDS系统分别设计成一个接收机钟差。

即其设计矩阵和待估状态量如下:

A = [ G 1 m G 1 n G 1 p G 1 − 1 0 G 2 m G 2 n G 2 p G 2 − 1 0 G 3 m G 3 n G 3 p G 3 − 1 0 G n m G n n G n p G n − 1 0 B 1 m B 1 n B 1 p B 1 0 − 1 B 2 m B 2 n B 2 p B 2 0 − 1 B 3 m B 3 n B 3 p B 3 0 − 1 B n m B n n B n p B n 0 − 1 ] A = \begin{bmatrix} G^1 & m_{G1} & n_{G1} & p_{G1} & -1 & 0 \\ G^2 & m_{G2} & n_{G2} & p_{G2} & -1 & 0 \\ G^3 & m_{G3} & n_{G3} & p_{G3} & -1 & 0 \\ G^n & m_{Gn} & n_{Gn} & p_{Gn} & -1 & 0 \\ B^1 & m_{B1} & n_{B1} & p_{B1} & 0 & -1 \\ B^2 & m_{B2} & n_{B2} & p_{B2} & 0 & -1 \\ B^3 & m_{B3} & n_{B3} & p_{B3} & 0 & -1 \\ B^n & m_{Bn} & n_{Bn} & p_{Bn} & 0 & -1 \end{bmatrix} A= ​G1G2G3GnB1B2B3Bn​mG1​mG2​mG3​mGn​mB1​mB2​mB3​mBn​​nG1​nG2​nG3​nGn​nB1​nB2​nB3​nBn​​pG1​pG2​pG3​pGn​pB1​pB2​pB3​pBn​​−1−1−1−10000​0000−1−1−1−1​ ​

δ z = [ d x d y d z c δ t G c δ t C ] \delta z = \begin{bmatrix} dx \\ dy \\ dz \\ c\delta t^G \\ c\delta t^C \end{bmatrix} δz= ​dxdydzcδtGcδtC​ ​

至于再增加GAL/GLO系统,原理一致,不再展开。

实例

假设我们有以下GPS和BDS两个系统的伪距观测值 P G 1 , P G 2 , P G 3 , P G n , P B 1 , P B 2 , P B 3 , P B n P_{G1}, P_{G2}, P_{G3}, P_{Gn}, P_{B1}, P_{B2}, P_{B3}, P_{Bn} PG1​,PG2​,PG3​,PGn​,PB1​,PB2​,PB3​,PBn​,以及它们的坐标 ( X G 1 , Y G 1 , Z G 1 ) , ( X G 2 , Y G 2 , Z G 2 ) , ( X G 3 , Y G 3 , Z G 3 ) , ( X G n , Y G n , Z G n ) , ( X B 1 , Y B 1 , Z B 1 ) , ( X B 2 , Y B 2 , Z B 2 ) , ( X B 3 , Y B 3 , Z B 3 ) , ( X B n , Y B n , Z B n ) (X_{G1}, Y_{G1}, Z_{G1}), (X_{G2}, Y_{G2}, Z_{G2}), (X_{G3}, Y_{G3}, Z_{G3}), (X_{Gn}, Y_{Gn}, Z_{Gn}), (X_{B1}, Y_{B1}, Z_{B1}), (X_{B2}, Y_{B2}, Z_{B2}), (X_{B3}, Y_{B3}, Z_{B3}), (X_{Bn}, Y_{Bn}, Z_{Bn}) (XG1​,YG1​,ZG1​),(XG2​,YG2​,ZG2​),(XG3​,YG3​,ZG3​),(XGn​,YGn​,ZGn​),(XB1​,YB1​,ZB1​),(XB2​,YB2​,ZB2​),(XB3​,YB3​,ZB3​),(XBn​,YBn​,ZBn​) 和接收机的概略位置 ( X 0 , Y 0 , Z 0 ) (X_0, Y_0, Z_0) (X0​,Y0​,Z0​)。

  1. 计算每个卫星到接收机概略位置的距离:

    计算从接收机的概略位置到每个卫星的距离,公式如下:

    R G 0 1 = ( X G 1 − X 0 ) 2 + ( Y G 1 − Y 0 ) 2 + ( Z G 1 − Z 0 ) 2 R_{G0}^1 = \sqrt{(X_{G1} - X_0)^2 + (Y_{G1} - Y_0)^2 + (Z_{G1} - Z_0)^2} RG01​=(XG1​−X0​)2+(YG1​−Y0​)2+(ZG1​−Z0​)2 ​

    R G 0 2 = ( X G 2 − X 0 ) 2 + ( Y G 2 − Y 0 ) 2 + ( Z G 2 − Z 0 ) 2 R_{G0}^2 = \sqrt{(X_{G2} - X_0)^2 + (Y_{G2} - Y_0)^2 + (Z_{G2} - Z_0)^2} RG02​=(XG2​−X0​)2+(YG2​−Y0​)2+(ZG2​−Z0​)2 ​

    R G 0 3 = ( X G 3 − X 0 ) 2 + ( Y G 3 − Y 0 ) 2 + ( Z G 3 − Z 0 ) 2 R_{G0}^3 = \sqrt{(X_{G3} - X_0)^2 + (Y_{G3} - Y_0)^2 + (Z_{G3} - Z_0)^2} RG03​=(XG3​−X0​)2+(YG3​−Y0​)2+(ZG3​−Z0​)2 ​

    R G 0 n = ( X G n − X 0 ) 2 + ( Y G n − Y 0 ) 2 + ( Z G n − Z 0 ) 2 R_{G0}^n = \sqrt{(X_{Gn} - X_0)^2 + (Y_{Gn} - Y_0)^2 + (Z_{Gn} - Z_0)^2} RG0n​=(XGn​−X0​)2+(YGn​−Y0​)2+(ZGn​−Z0​)2 ​

    R B 0 1 = ( X B 1 − X 0 ) 2 + ( Y B 1 − Y 0 ) 2 + ( Z B 1 − Z 0 ) 2 R_{B0}^1 = \sqrt{(X_{B1} - X_0)^2 + (Y_{B1} - Y_0)^2 + (Z_{B1} - Z_0)^2} RB01​=(XB1​−X0​)2+(YB1​−Y0​)2+(ZB1​−Z0​)2 ​

    R B 0 2 = ( X B 2 − X 0 ) 2 + ( Y B 2 − Y 0 ) 2 + ( Z B 2 − Z 0 ) 2 R_{B0}^2 = \sqrt{(X_{B2} - X_0)^2 + (Y_{B2} - Y_0)^2 + (Z_{B2} - Z_0)^2} RB02​=(XB2​−X0​)2+(YB2​−Y0​)2+(ZB2​−Z0​)2 ​

    R B 0 3 = ( X B 3 − X 0 ) 2 + ( Y B 3 − Y 0 ) 2 + ( Z B 3 − Z 0 ) 2 R_{B0}^3 = \sqrt{(X_{B3} - X_0)^2 + (Y_{B3} - Y_0)^2 + (Z_{B3} - Z_0)^2} RB03​=(XB3​−X0​)2+(YB3​−Y0​)2+(ZB3​−Z0​)2 ​

    R B 0 n = ( X B n − X 0 ) 2 + ( Y B n − Y 0 ) 2 + ( Z B n − Z 0 ) 2 R_{B0}^n = \sqrt{(X_{Bn} - X_0)^2 + (Y_{Bn} - Y_0)^2 + (Z_{Bn} - Z_0)^2} RB0n​=(XBn​−X0​)2+(YBn​−Y0​)2+(ZBn​−Z0​)2 ​

  2. 计算方向余弦:

    方向余弦用于表示从接收机概略位置到每个卫星的方向向量。计算如下:

    m G 1 = X G 1 − X 0 R G 0 1 , n G 1 = Y G 1 − Y 0 R G 0 1 , p G 1 = Z G 1 − Z 0 R G 0 1 m_{G1} = \frac{X_{G1} - X_0}{R_{G0}^1}, \quad n_{G1} = \frac{Y_{G1} - Y_0}{R_{G0}^1}, \quad p_{G1} = \frac{Z_{G1} - Z_0}{R_{G0}^1} mG1​=RG01​XG1​−X0​​,nG1​=RG01​YG1​−Y0​​,pG1​=RG01​ZG1​−Z0​​

    m G 2 = X G 2 − X 0 R G 0 2 , n G 2 = Y G 2 − Y 0 R G 0 2 , p G 2 = Z G 2 − Z 0 R G 0 2 m_{G2} = \frac{X_{G2} - X_0}{R_{G0}^2}, \quad n_{G2} = \frac{Y_{G2} - Y_0}{R_{G0}^2}, \quad p_{G2} = \frac{Z_{G2} - Z_0}{R_{G0}^2} mG2​=RG02​XG2​−X0​​,nG2​=RG02​YG2​−Y0​​,pG2​=RG02​ZG2​−Z0​​

    m G 3 = X G 3 − X 0 R G 0 3 , n G 3 = Y G 3 − Y 0 R G 0 3 , p G 3 = Z G 3 − Z 0 R G 0 3 m_{G3} = \frac{X_{G3} - X_0}{R_{G0}^3}, \quad n_{G3} = \frac{Y_{G3} - Y_0}{R_{G0}^3}, \quad p_{G3} = \frac{Z_{G3} - Z_0}{R_{G0}^3} mG3​=RG03​XG3​−X0​​,nG3​=RG03​YG3​−Y0​​,pG3​=RG03​ZG3​−Z0​​

    m G n = X G n − X 0 R G 0 n , n G n = Y G n − Y 0 R G 0 n , p G n = Z G n − Z 0 R G 0 n m_{Gn} = \frac{X_{Gn} - X_0}{R_{G0}^n}, \quad n_{Gn} = \frac{Y_{Gn} - Y_0}{R_{G0}^n}, \quad p_{Gn} = \frac{Z_{Gn} - Z_0}{R_{G0}^n} mGn​=RG0n​XGn​−X0​​,nGn​=RG0n​YGn​−Y0​​,pGn​=RG0n​ZGn​−Z0​​

    m B 1 = X B 1 − X 0 R B 0 1 , n B 1 = Y B 1 − Y 0 R B 0 1 , p B 1 = Z B 1 − Z 0 R B 0 1 m_{B1} = \frac{X_{B1} - X_0}{R_{B0}^1}, \quad n_{B1} = \frac{Y_{B1} - Y_0}{R_{B0}^1}, \quad p_{B1} = \frac{Z_{B1} - Z_0}{R_{B0}^1} mB1​=RB01​XB1​−X0​​,nB1​=RB01​YB1​−Y0​​,pB1​=RB01​ZB1​−Z0​​

    m B 2 = X B 2 − X 0 R B 0 2 , n B 2 = Y B 2 − Y 0 R B 0 2 , p B 2 = Z B 2 − Z 0 R B 0 2 m_{B2} = \frac{X_{B2} - X_0}{R_{B0}^2}, \quad n_{B2} = \frac{Y_{B2} - Y_0}{R_{B0}^2}, \quad p_{B2} = \frac{Z_{B2} - Z_0}{R_{B0}^2} mB2​=RB02​XB2​−X0​​,nB2​=RB02​YB2​−Y0​​,pB2​=RB02​ZB2​−Z0​​

    m B 3 = X B 3 − X 0 R B 0 3 , n B 3 = Y B 3 − Y 0 R B 0 3 , p B 3 = Z B 3 − Z 0 R B 0 3 m_{B3} = \frac{X_{B3} - X_0}{R_{B0}^3}, \quad n_{B3} = \frac{Y_{B3} - Y_0}{R_{B0}^3}, \quad p_{B3} = \frac{Z_{B3} - Z_0}{R_{B0}^3} mB3​=RB03​XB3​−X0​​,nB3​=RB03​YB3​−Y0​​,pB3​=RB03​ZB3​−Z0​​

    m B n = X B n − X 0 R B 0 n , n B n = Y B n − Y 0 R B 0 n , p B n = Z B n − Z 0 R B 0 n m_{Bn} = \frac{X_{Bn} - X_0}{R_{B0}^n}, \quad n_{Bn} = \frac{Y_{Bn} - Y_0}{R_{B0}^n}, \quad p_{Bn} = \frac{Z_{Bn} - Z_0}{R_{B0}^n} mBn​=RB0n​XBn​−X0​​,nBn​=RB0n​YBn​−Y0​​,pBn​=RB0n​ZBn​−Z0​​

  3. 形成设计矩阵 A A A 和先验残差矩阵 V V V:

    设计矩阵 A A A 和先验残差矩阵 V V V 用于表示伪距观测方程的线性形式:

    A = [ m G 1 n G 1 p G 1 − 1 0 m G 2 n G 2 p G 2 − 1 0 m G 3 n G 3 p G 3 − 1 0 m G n n G n p G n − 1 0 m B 1 n B 1 p B 1 0 − 1 m B 2 n B 2 p B 2 0 − 1 m B 3 n B 3 p B 3 0 − 1 m B n n B n p B n 0 − 1 ] A = \begin{bmatrix} m_{G1} & n_{G1} & p_{G1} & -1 & 0 \\ m_{G2} & n_{G2} & p_{G2} & -1 & 0 \\ m_{G3} & n_{G3} & p_{G3} & -1 & 0 \\ m_{Gn} & n_{Gn} & p_{Gn} & -1 & 0 \\ m_{B1} & n_{B1} & p_{B1} & 0 & -1 \\ m_{B2} & n_{B2} & p_{B2} & 0 & -1 \\ m_{B3} & n_{B3} & p_{B3} & 0 & -1 \\ m_{Bn} & n_{Bn} & p_{Bn} & 0 & -1 \end{bmatrix} A= ​mG1​mG2​mG3​mGn​mB1​mB2​mB3​mBn​​nG1​nG2​nG3​nGn​nB1​nB2​nB3​nBn​​pG1​pG2​pG3​pGn​pB1​pB2​pB3​pBn​​−1−1−1−10000​0000−1−1−1−1​ ​

    先验残差矩阵 V V V 计算如下:

    V = [ l G 1 l G 2 l G 3 l G n l B 1 l B 2 l B 3 l B n ] = [ P G 1 − R G 0 1 + c δ t G − I G 1 − T G 1 − ϵ P G 1 P G 2 − R G 0 2 + c δ t G − I G 2 − T G 2 − ϵ P G 2 P G 3 − R G 0 3 + c δ t G − I G 3 − T G 3 − ϵ P G 3 P G n − R G 0 n + c δ t G − I G n − T G n − ϵ P G n P B 1 − R B 0 1 + c δ t C − I B 1 − T B 1 − ϵ P B 1 P B 2 − R B 0 2 + c δ t C − I B 2 − T B 2 − ϵ P B 2 P B 3 − R B 0 3 + c δ t C − I B 3 − T B 3 − ϵ P B 3 P B n − R B 0 n + c δ t C − I B n − T B n − ϵ P B n ] V = \begin{bmatrix} l_{G1} \\ l_{G2} \\ l_{G3} \\ l_{Gn} \\ l_{B1} \\ l_{B2} \\ l_{B3} \\ l_{Bn} \end{bmatrix} = \begin{bmatrix} P_{G1} - R_{G0}^1 + c\delta t^G - I_{G1} - T_{G1} - \epsilon_{P_{G1}} \\ P_{G2} - R_{G0}^2 + c\delta t^G - I_{G2} - T_{G2} - \epsilon_{P_{G2}} \\ P_{G3} - R_{G0}^3 + c\delta t^G - I_{G3} - T_{G3} - \epsilon_{P_{G3}} \\ P_{Gn} - R_{G0}^n + c\delta t^G - I_{Gn} - T_{Gn} - \epsilon_{P_{Gn}} \\ P_{B1} - R_{B0}^1 + c\delta t^C - I_{B1} - T_{B1} - \epsilon_{P_{B1}} \\ P_{B2} - R_{B0}^2 + c\delta t^C - I_{B2} - T_{B2} - \epsilon_{P_{B2}} \\ P_{B3} - R_{B0}^3 + c\delta t^C - I_{B3} - T_{B3} - \epsilon_{P_{B3}} \\ P_{Bn} - R_{B0}^n + c\delta t^C - I_{Bn} - T_{Bn} - \epsilon_{P_{Bn}} \end{bmatrix} V= ​lG1​lG2​lG3​lGn​lB1​lB2​lB3​lBn​​ ​= ​PG1​−RG01​+cδtG−IG1​−TG1​−ϵPG1​​PG2​−RG02​+cδtG−IG2​−TG2​−ϵPG2​​PG3​−RG03​+cδtG−IG3​−TG3​−ϵPG3​​PGn​−RG0n​+cδtG−IGn​−TGn​−ϵPGn​​PB1​−RB01​+cδtC−IB1​−TB1​−ϵPB1​​PB2​−RB02​+cδtC−IB2​−TB2​−ϵPB2​​PB3​−RB03​+cδtC−IB3​−TB3​−ϵPB3​​PBn​−RB0n​+cδtC−IBn​−TBn​−ϵPBn​​​ ​

    这里的 l i l_i li​ 表示每个伪距观测值减去计算得到的几何距离并进行误差修正后的值。

  4. 解方程 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz 以估计状态量 δ z = [ d x d y d z c δ t G c δ t C ] T \delta z = \begin{bmatrix} dx & dy & dz & c\delta t^G & c\delta t^C \end{bmatrix}^T δz=[dx​dy​dz​cδtG​cδtC​]T:

    通过求解线性方程组 V = − A ⋅ δ z V = -A \cdot \delta z V=−A⋅δz,我们可以估计状态量 δ z \delta z δz,即接收机位置的修正量和接收机钟差:

    δ z = ( A T A ) − 1 A T V \delta z = (A^T A)^{-1} A^T V δz=(ATA)−1ATV

    这里, ( A T A ) − 1 A T (A^T A)^{-1} A^T (ATA)−1AT 是伪逆矩阵,用于求解最小二乘问题。

  5. 更新接收机位置:

    根据估计的修正量,更新接收机的位置:

    X r = X 0 + d x , Y r = Y 0 + d y , Z r = Z 0 + d z X_r = X_0 + dx, \quad Y_r = Y_0 + dy, \quad Z_r = Z_0 + dz Xr​=X0​+dx,Yr​=Y0​+dy,Zr​=Z0​+dz

    最终,我们得到了更精确的接收机位置 ( X r , Y r , Z r ) (X_r, Y_r, Z_r) (Xr​,Yr​,Zr​)。

通过以上步骤,可以得到接收机的精确位置偏差,并更新接收机的概略位置,从而得到更精确的定位结果。

9.4 定位流程

在这里插入图片描述


微信公众号

今日签到

点亮在社区的每一天
去签到