GPS從入門到放棄(十) — 定位方程解算和定位精度
上一篇僞距與載波相位 中我們介紹了僞距的計算方法,也得到了包含 ( x , y , z , δ t ) (x,\ y,\ z,\ \delta_t) ( x , y , z , δ t ) 四個未知數的GPS定位基本方程:
( x − x s ) 2 + ( y − y s ) 2 + ( z − z s ) 2 + c ⋅ δ t = ρ + c ⋅ δ t , s − c I − c T − c ϵ \sqrt{(x-x_{s})^2 + (y-y_{s})^2 + (z-z_{s})^2} + c\cdot\delta_t = \rho + c\cdot\delta_{t,s} - cI - cT -c\epsilon ( x − x s ) 2 + ( y − y s ) 2 + ( z − z s ) 2 + c ⋅ δ t = ρ + c ⋅ δ t , s − c I − c T − c ϵ
那麼根據這個方程我們怎麼來定位呢?
這個方程中的 I I I 和 T T T 分別是大氣電離層導致的延時和大氣對流層導致的延時,這些延時的計算方法放到後面再講,目前我們先把它當作已知量。δ t , s \delta_{t,s} δ t , s 爲衛星鐘差,在導航電文中有參數修正,以後再講。於是這個方程中只涉及到四個未知數和一個誤差。我們先考慮簡單的情況,即暫時不管誤差 ϵ \epsilon ϵ ,在分析定位精度的時候再來考慮它。
根據我們第一篇GPS基礎原理 講過GPS的基本原理,只需已知四顆衛星的測量值,即可組成一個四元方程組,然後解出來這四個未知數。要注意的是這個方程組是一個非線性方程組,因此在實際解算過程中,常用牛頓迭代法來進行。
牛頓迭代法
牛頓迭代法是一個常用的解非線性方程組的方法,它將非線性方程組在一個估計解的附近進行線性化,然後求解線性化後的方程組,接着再更新解的估計值。如此反覆迭代,直到解的精度滿足要求爲止。
根據牛頓迭代法,將四元方程組在第k次迭代的估計解 [ x k y k z k δ t , k ] T [x_k \ \ y_k \ \ z_k\ \ \delta_{t,k}]^T [ x k y k z k δ t , k ] T 處線性化後方程組爲:
G [ Δ x Δ y Δ z c ⋅ Δ δ t ] = b \boldsymbol{G} \left[ \begin{array}{c}
\Delta x\\
\Delta y\\
\Delta z\\
c\cdot\Delta \delta_{t}
\end{array}
\right] = \boldsymbol{b}
G ⎣ ⎢ ⎢ ⎡ Δ x Δ y Δ z c ⋅ Δ δ t ⎦ ⎥ ⎥ ⎤ = b
其中
G = [ x k − x s , 1 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 y k − y s , 1 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 z k − z s , 1 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 1 x k − x s , 2 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 y k − y s , 2 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 z k − z s , 2 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 1 x k − x s , 3 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 y k − y s , 3 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 z k − z s , 3 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 1 x k − x s , 4 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 y k − y s , 4 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 z k − z s , 4 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 1 ] \boldsymbol{G} = \left[ \begin{array}{cccc}
\frac{x_k-x_{s,1}}{\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2}} &
\frac{y_k-y_{s,1}}{\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2}} &
\frac{z_k-z_{s,1}}{\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2}} & 1\\
\frac{x_k-x_{s,2}}{\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2}} &
\frac{y_k-y_{s,2}}{\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2}} &
\frac{z_k-z_{s,2}}{\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2}} & 1\\
\frac{x_k-x_{s,3}}{\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2}} &
\frac{y_k-y_{s,3}}{\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2}} &
\frac{z_k-z_{s,3}}{\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2}} & 1\\
\frac{x_k-x_{s,4}}{\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2}} &
\frac{y_k-y_{s,4}}{\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2}} &
\frac{z_k-z_{s,4}}{\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2}} & 1
\end{array}
\right]
G = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 x k − x s , 1 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 x k − x s , 2 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 x k − x s , 3 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 x k − x s , 4 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 y k − y s , 1 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 y k − y s , 2 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 y k − y s , 3 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 y k − y s , 4 ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 z k − z s , 1 ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 z k − z s , 2 ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 z k − z s , 3 ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 z k − z s , 4 1 1 1 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤
b = [ ρ 1 + c ⋅ δ t , s , 1 − c I 1 − c T 1 − ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 − c ⋅ δ t , k ρ 2 + c ⋅ δ t , s , 2 − c I 2 − c T 2 − ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 − c ⋅ δ t , k ρ 3 + c ⋅ δ t , s , 3 − c I 3 − c T 3 − ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 − c ⋅ δ t , k ρ 4 + c ⋅ δ t , s , 4 − c I 4 − c T 4 − ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 − c ⋅ δ t , k ]
\boldsymbol{b} = \left[ \begin{array}{cccc}
\rho_1 + c\cdot\delta_{t,s,1} - cI_1 - cT_1 -\sqrt{(x_k-x_{s,1})^2 + (y_k-y_{s,1})^2 + (z_k-z_{s,1})^2} - c\cdot\delta_{t,k} \\
\rho_2 + c\cdot\delta_{t,s,2} - cI_2 - cT_2 -\sqrt{(x_k-x_{s,2})^2 + (y_k-y_{s,2})^2 + (z_k-z_{s,2})^2} - c\cdot\delta_{t,k} \\
\rho_3 + c\cdot\delta_{t,s,3} - cI_3 - cT_3 -\sqrt{(x_k-x_{s,3})^2 + (y_k-y_{s,3})^2 + (z_k-z_{s,3})^2} - c\cdot\delta_{t,k} \\
\rho_4 + c\cdot\delta_{t,s,4} - cI_4 - cT_4 -\sqrt{(x_k-x_{s,4})^2 + (y_k-y_{s,4})^2 + (z_k-z_{s,4})^2} - c\cdot\delta_{t,k}
\end{array}
\right]
b = ⎣ ⎢ ⎢ ⎡ ρ 1 + c ⋅ δ t , s , 1 − c I 1 − c T 1 − ( x k − x s , 1 ) 2 + ( y k − y s , 1 ) 2 + ( z k − z s , 1 ) 2 − c ⋅ δ t , k ρ 2 + c ⋅ δ t , s , 2 − c I 2 − c T 2 − ( x k − x s , 2 ) 2 + ( y k − y s , 2 ) 2 + ( z k − z s , 2 ) 2 − c ⋅ δ t , k ρ 3 + c ⋅ δ t , s , 3 − c I 3 − c T 3 − ( x k − x s , 3 ) 2 + ( y k − y s , 3 ) 2 + ( z k − z s , 3 ) 2 − c ⋅ δ t , k ρ 4 + c ⋅ δ t , s , 4 − c I 4 − c T 4 − ( x k − x s , 4 ) 2 + ( y k − y s , 4 ) 2 + ( z k − z s , 4 ) 2 − c ⋅ δ t , k ⎦ ⎥ ⎥ ⎤
我們將 G \boldsymbol{G} G 稱爲雅可比矩陣。
設第k次迭代時接收機到衛星 s s s 的單位觀測矢量爲 e s , k = [ e s , k , x , e s , k , y , e s , k , z ] T \boldsymbol{e}_{s,k}=[e_{s,k,x},\ e_{s,k,y},\ e_{s,k,z}]^T e s , k = [ e s , k , x , e s , k , y , e s , k , z ] T ,則 G \boldsymbol{G} G 可以寫爲:
G = [ − e 1 , k , x − e 1 , k , y − e 1 , k , z 1 − e 2 , k , x − e 2 , k , y − e 2 , k , z 1 − e 3 , k , x − e 3 , k , y − e 3 , k , z 1 − e 4 , k , x − e 4 , k , y − e 4 , k , z 1 ] = [ − e 1 , k 1 − e 2 , k 1 − e 3 , k 1 − e 4 , k 1 ]
\boldsymbol{G} = \left[ \begin{array}{cccc}
-e_{1,k,x} & -e_{1,k,y} & -e_{1,k,z} & 1\\
-e_{2,k,x} & -e_{2,k,y} & -e_{2,k,z} & 1\\
-e_{3,k,x} & -e_{3,k,y} & -e_{3,k,z} & 1\\
-e_{4,k,x} & -e_{4,k,y} & -e_{4,k,z} & 1
\end{array}
\right] = \left[ \begin{array}{cc}
-\boldsymbol{e}_{1,k} & 1\\
-\boldsymbol{e}_{2,k} & 1\\
-\boldsymbol{e}_{3,k} & 1\\
-\boldsymbol{e}_{4,k} & 1
\end{array}
\right]
G = ⎣ ⎢ ⎢ ⎡ − e 1 , k , x − e 2 , k , x − e 3 , k , x − e 4 , k , x − e 1 , k , y − e 2 , k , y − e 3 , k , y − e 4 , k , y − e 1 , k , z − e 2 , k , z − e 3 , k , z − e 4 , k , z 1 1 1 1 ⎦ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎡ − e 1 , k − e 2 , k − e 3 , k − e 4 , k 1 1 1 1 ⎦ ⎥ ⎥ ⎤
觀察 G \boldsymbol{G} G 可以發現,G \boldsymbol{G} G 只與衛星和接收機的幾何位置有關,所以也稱 G \boldsymbol{G} G 爲幾何矩陣。
而一般把 b \boldsymbol{b} b 稱爲僞距殘差。它是觀測到的僞距與第k次迭代時估計出的僞距的差值。
得到線性化的方程組之後,我們就可以用最小二乘法將方程組解出來,得到
[ Δ x Δ y Δ z c ⋅ Δ δ t ] = ( G T G ) − 1 G T b \left[ \begin{array}{c}
\Delta x\\
\Delta y\\
\Delta z\\
c\cdot\Delta \delta_{t}
\end{array}
\right] = (\boldsymbol{G}^T\boldsymbol{G})^{-1}\boldsymbol{G}^T\boldsymbol{b}
⎣ ⎢ ⎢ ⎡ Δ x Δ y Δ z c ⋅ Δ δ t ⎦ ⎥ ⎥ ⎤ = ( G T G ) − 1 G T b
然後進一步得到迭代下一步的估計值
[ x k + 1 y k + 1 z k + 1 δ t , k + 1 ] = [ x k + Δ x y k + Δ y z k + Δ z δ t , k + Δ δ t ] \left[ \begin{array}{c}
x_{k+1}\\
y_{k+1}\\
z_{k+1}\\
\delta_{t,k+1}
\end{array}
\right] = \left[ \begin{array}{c}
x_{k} + \Delta x\\
y_{k} + \Delta y\\
z_{k} + \Delta z\\
\delta_{t,k} + \Delta \delta_{t}
\end{array}
\right]
⎣ ⎢ ⎢ ⎡ x k + 1 y k + 1 z k + 1 δ t , k + 1 ⎦ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎡ x k + Δ x y k + Δ y z k + Δ z δ t , k + Δ δ t ⎦ ⎥ ⎥ ⎤
如此反覆迭代,直到 [ x k y k z k δ t , k ] T [x_k \ \ y_k \ \ z_k\ \ \delta_{t,k}]^T [ x k y k z k δ t , k ] T 滿足精度要求,牛頓迭代法即可中止。
在使用牛頓迭代法解算位置的過程中,需要注意幾點:
是否收斂。解的估計值有可能在一個值附近來回振盪,這是無法得到更高精度的解。
是否收斂到地球附近位置。解有可能收斂到遠離地球的一端,這時需要重新給初始值,重新進行迭代解算。
嚴格來講,每次迭代位置更新後,大氣延時等誤差需要重新估算,爲了減小計算量,在連續定位時可以認爲此誤差在迭代中保持不變。
若可觀測衛星多於4顆,可以對雅可比矩陣 G \boldsymbol{G} G 進行擴展,依然可以用牛頓迭代和最小二乘法來求解。
若可觀測衛星少於4顆,可以利用各種假設來增加輔助方程,以解出需要的未知數。如限定高度變化量、限定運動方向、限定接收機鐘差變化量等,當然此處需要注意限定的有效期。
定位精度
下面我們把誤差也考慮進去,假定測量誤差和定位誤差都很小,於是線性化後方程組爲:
G [ Δ x + ϵ x Δ y + ϵ y Δ z + ϵ z c ⋅ ( Δ δ t + ϵ δ t ) ] = b + ϵ ρ \boldsymbol{G} \left[ \begin{array}{c}
\Delta x + \epsilon_x\\
\Delta y + \epsilon_y\\
\Delta z + \epsilon_z\\
c\cdot(\Delta \delta_{t} + \epsilon_{\delta_{t}})
\end{array}
\right] = \boldsymbol{b + \epsilon_\rho}
G ⎣ ⎢ ⎢ ⎡ Δ x + ϵ x Δ y + ϵ y Δ z + ϵ z c ⋅ ( Δ δ t + ϵ δ t ) ⎦ ⎥ ⎥ ⎤ = b + ϵ ρ
其中
ϵ ρ = [ ϵ ρ , 1 ϵ ρ , 2 ϵ ρ , 3 ϵ ρ , 4 ]
\boldsymbol{\epsilon_\rho} = \left[ \begin{array}{c}
\epsilon_{\rho,1}\\
\epsilon_{\rho,2}\\
\epsilon_{\rho,3}\\
\epsilon_{\rho,4}
\end{array}
\right]
ϵ ρ = ⎣ ⎢ ⎢ ⎡ ϵ ρ , 1 ϵ ρ , 2 ϵ ρ , 3 ϵ ρ , 4 ⎦ ⎥ ⎥ ⎤
爲衛星的測量誤差向量,ϵ x , ϵ y , ϵ z \epsilon_x, \epsilon_y, \epsilon_z ϵ x , ϵ y , ϵ z 和 ϵ δ t \epsilon_{\delta_t} ϵ δ t 分別表示由誤差向量 ϵ ρ \boldsymbol{\epsilon_\rho} ϵ ρ 引起的定位和定時誤差。
解這個方程可以得到
[ ϵ x ϵ y ϵ z ϵ δ t ] = ( G T G ) − 1 G T ϵ ρ \left[ \begin{array}{c}
\epsilon_x\\
\epsilon_y\\
\epsilon_z\\
\epsilon_{\delta_{t}}
\end{array}
\right] = (\boldsymbol{G}^T\boldsymbol{G})^{-1}\boldsymbol{G}^T\boldsymbol{\epsilon_\rho}
⎣ ⎢ ⎢ ⎡ ϵ x ϵ y ϵ z ϵ δ t ⎦ ⎥ ⎥ ⎤ = ( G T G ) − 1 G T ϵ ρ
假設各個衛星的測量誤差都爲正態分佈,其均值 E [ ϵ ρ ] = 0 E[\epsilon_\rho] = 0 E [ ϵ ρ ] = 0 ,方差 V [ ϵ ρ ] = σ U R E 2 V[\epsilon_\rho] = \sigma_{URE}^2 V [ ϵ ρ ] = σ U R E 2 ,假設各個衛星的測量誤差互不相關,則可以推導出定位誤差協方差陣爲:
C o v ( [ ϵ x ϵ y ϵ z ϵ δ t ] ) = ( G T G ) − 1 σ U R E 2 = H σ U R E 2
Cov\left(\left[ \begin{array}{c}
\epsilon_x\\
\epsilon_y\\
\epsilon_z\\
\epsilon_{\delta_{t}}
\end{array}
\right]\right)
= (\boldsymbol{G}^T\boldsymbol{G})^{-1}\sigma_{URE}^2
= \boldsymbol{H}\sigma_{URE}^2
C o v ⎝ ⎜ ⎜ ⎛ ⎣ ⎢ ⎢ ⎡ ϵ x ϵ y ϵ z ϵ δ t ⎦ ⎥ ⎥ ⎤ ⎠ ⎟ ⎟ ⎞ = ( G T G ) − 1 σ U R E 2 = H σ U R E 2
其中
H = ( G T G ) − 1
\boldsymbol{H} = (\boldsymbol{G}^T\boldsymbol{G})^{-1}
H = ( G T G ) − 1
爲權係數陣,是一個對稱陣。
由定位誤差協方差陣可以看出,GPS定位誤差的方差是測量誤差的方差被權係數陣放大的結果,而權係數陣只與衛星的幾何分佈有關,故GPS的定位誤差取決於測量誤差和衛星幾何分佈兩個因素。
精度因子
有了權係數陣,我們就可以計算精度因子了。精度因子用於表示各個方向和時鐘的誤差放大倍數。假設在站心座標系(座標系可參見前文GPS座標系 )下表示的權係數陣爲:
H = [ h 11 h 22 h 33 h 44 ]
\boldsymbol{H} = \left[ \begin{array}{cccc}
h_{11} & & & \\
& h_{22} & & \\
& & h_{33} & \\
& & & h_{44}
\end{array}
\right]
H = ⎣ ⎢ ⎢ ⎡ h 1 1 h 2 2 h 3 3 h 4 4 ⎦ ⎥ ⎥ ⎤
那麼水平精度因子(HDOP)、高程精度因子(VDOP)、位置精度因子(PDOP)、鐘差精度因子(TDOP)、幾何精度因子(GDOP)分別爲:
H D O P = h 11 2 + h 22 2 V D O P = h 33 2 P D O P = h 11 2 + h 22 2 + h 33 2 T D O P = h 44 2 G D O P = h 11 2 + h 22 2 + h 33 2 + h 44 2 \begin{array}{c}
HDOP = \sqrt{h_{11}^2 + h_{22}^2} \\
VDOP = \sqrt{h_{33}^2} \\
PDOP = \sqrt{h_{11}^2 + h_{22}^2 + h_{33}^2} \\
TDOP = \sqrt{h_{44}^2} \\
GDOP = \sqrt{h_{11}^2 + h_{22}^2 + h_{33}^2 + h_{44}^2}
\end{array}
H D O P = h 1 1 2 + h 2 2 2 V D O P = h 3 3 2 P D O P = h 1 1 2 + h 2 2 2 + h 3 3 2 T D O P = h 4 4 2 G D O P = h 1 1 2 + h 2 2 2 + h 3 3 2 + h 4 4 2
一般GPS接收機在輸出定位結果的同時都會輸出精度因子,在相同測量誤差的情況下,精度因子越小,定位精度越高。
精度因子只與衛星的幾何分佈有關,有一個簡單的方法可以大致判斷GDOP的大小:以接收機所在位置爲錐頂、以各個衛星所在位置爲頂點組成一個錐形體,這個錐形體體積越大,相應的GDOP就越小。