蔣長輝，陳育偉，Juha Hyyppa

(芬蘭地球空間研究所遙感與攝影測量部, 芬蘭 埃斯波 FI-02450)

0 引言

芯片級全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)接收機一直是智能手機的重要組成部分[1]。智能手機GNSS接收機可以提供三維位置信息，支持各種基于位置的服務(wù)應(yīng)用(Locat-ion-Based-Service，LBS)[1]。然而，在以高樓為特征的城市地區(qū)，受天線限制，智能手機GNSS信號質(zhì)量較差，在信號惡劣的環(huán)境下定位精度有時會下降。具體來說，城市地區(qū)的GNSS信號傳輸路徑受多徑(Multi-Path，MP)、非視距(None-Line-Of-Sight，NLOS)信號等多種因素的影響。MP是指GNSS接收機同時收到了反射信號和直射信號，兩個信號混和在一起；NLOS是指GNSS接收機接收到反射信號，而沒有接收到直射信號。額外的信號傳輸路徑會引起高達數(shù)十米的位置偏差，這是導(dǎo)致城市地區(qū)智能手機GNSS定位精度降低的主要因素[2-4]。

在智能手機GNSS接收機中，通常使用最小二乘法(Least Square，LS)完成位置、速度和時間(Position Velocity and Time，PVT)的計算[5]。在 LS 方法中，PVT的計算是基于最小化測量殘差的規(guī)則進行的。在城市復(fù)雜環(huán)境下，NLOS造成的偽距偏差會污染測量并導(dǎo)致位置偏差[4]。就其本質(zhì)而言，LS方法忽略了位置和速度之間的內(nèi)在關(guān)系。事實上，在NLOS條件下，速度更值得信賴，可以用來平滑位置?？柭鼮V波器(Kalman Filter，KF)是計算 PVT信息的另一種方法，其規(guī)則是最小化均方誤差[5]。KF相對于LS而言，結(jié)合運動模型利用速度信息對位置信息進行平滑。具體來說，首先構(gòu)建位置-速度狀態(tài)傳播模型，通過這種方式利用速度信息平滑位置估計[6]。由于信號的反射對導(dǎo)航信號多普勒頻移影響相對更小，速度比位置更準確，在KF中采用狀態(tài)傳播模型的預(yù)測信息，期望得到更平滑和更準確的解。然而，KF 忽略了這些狀態(tài)的時間相關(guān)特性，這些狀態(tài)通過速度信息高度相關(guān)。KF以遞歸方式工作，在估計當前狀態(tài)時忽略過去的歷史信息[6]。

因子圖優(yōu)化(Factor Graph Optimization，F(xiàn)GO)方法在同步定位和制圖(Simultaneous Localization and Mapping，SLAM)中獲得了巨大的成功[7-10]。它利用所有與時間相關(guān)的過去測量一起估計所有過去的狀態(tài)，F(xiàn)GO算法在GNSS位置信息估計中具有巨大的潛力。T.Suzuki在 GNSS 時間相關(guān)實時動態(tài)(Real-Time Kinematic, RTK)中使用了FGO，并獲得了優(yōu)于KF的位置結(jié)果[7]。蔣長輝在GNSS矢量跟蹤(Vector Tracking，VT)中實現(xiàn)了FGO，通過在VT 導(dǎo)航濾波器中用FGO代替KF獲得了更好的定位結(jié)果[9]。此外，在 GNSS中研究了FGO，以提高城市地區(qū)位置的穩(wěn)健性[10-13]。然而，在減小NLOS引起的誤差方面，速度并未得到充分利用。尤其是智能手機GNSS，受限于硬件，研究利用速度平滑定位結(jié)果具有重要意義。

為了減少城市復(fù)雜環(huán)境下NLOS信號對智能手機 GNSS引起的位置誤差，本文提出了一種基于自適應(yīng)FGO的智能手機GNSS位置平滑方法，并通過實際場景實驗驗證了所提算法的性能。

1 模型和方法

(1)

式中，tb表示GNSS接收機時鐘鐘差；c表示光速。另外，式(1)中的偽距指矯正過之后的[6]。

1.1 LS

根據(jù)式(1)可以計算用戶位置，位置增量和偽距增量之間的關(guān)系可以表示為

(2)

(3)

在NLOS環(huán)境下，式(3)可以被寫成如下

(4)

式中，bNLOS為NLOS信號造成的位置偏差。通過對比式(3)和式(4)可以發(fā)現(xiàn)，NLOS信號導(dǎo)致了位置估計結(jié)果的額外偏置。

1.2 KF

KF狀態(tài)方程建立在用戶運動學模型之上，狀態(tài)方程如下

=Fk,k+1·Xk+wk+1

(5)

KF量測方程如下

=Hk+1·Xk+1+ηk+1

(6)

在NLOS環(huán)境下，將NLOS信號造成的偏置添加到量測信息，新的模型如下

(7)

KF更新過程如下

(8)

(9)

(10)

Kk+1=

(11)

(12)

式中，Qk和Rk+1為狀態(tài)方程噪聲協(xié)方差矩陣和量測噪聲協(xié)方差矩陣；Pk和Kk+1為誤差協(xié)方差矩陣和卡爾曼增益矩陣。

在NLOS環(huán)境下，式(10)可以寫成如下形式

(13)

從式(13)可以看出，NLOS信號從以下兩個方面對KF估計的狀態(tài)產(chǎn)生影響：1)NLOS信號產(chǎn)生的量測偏置在KF狀態(tài)更新時降低了狀態(tài)估計精度；2)信號經(jīng)過反射之后，量測噪聲變大，如果保持量測噪聲協(xié)方差不變會對狀態(tài)估計產(chǎn)生消極影響。

2 FGO方法

2.1 因子圖構(gòu)建和模型

在因子圖中，通常用邊連接因子節(jié)點和變量節(jié)點。對于給定的測量，狀態(tài)估計的目的是找到最佳的后驗狀態(tài)估計，可以寫成如下形式

(14)

(15)

如上所述，狀態(tài)估計是基于運動模型和測量模型進行的?；谒俣鹊臓顟B(tài)變換和位置測量被建模為兩個獨立的節(jié)點。假設(shè)狀態(tài)過程噪聲和測量噪聲均服從具有協(xié)方差矩陣的零均值高斯分布，可以將狀態(tài)估計表述為最大后驗問題，模型寫為

(16)

(17)

(18)

(19)

基于式(5)～式(7)列出的狀態(tài)和測量模型，F(xiàn)GO的圖形結(jié)構(gòu)如圖1所示。如前所述，這里有兩個因子：運動模型因子和GPS位置因子，詳細信息和誤差函數(shù)如下所示。

圖1 因子圖結(jié)構(gòu)Fig.1 Structure of the factor graph

(1)運動模型因子

兩個連續(xù)位置通過速度相關(guān)，基于運動模型，兩個連續(xù)位置表示為

(20)

因此，代價函數(shù)可以寫成如下

(21)

(2) GNSS因子

GNSS位置信息作為量測，模型如下

(22)

對應(yīng)的代價函數(shù)如下

(23)

綜上，位置的最優(yōu)估計可以轉(zhuǎn)換成最小化下列代價函數(shù)

(24)

本文中，采用GTSAM圖優(yōu)化庫求解狀態(tài)的最優(yōu)估計[14-16]。

2.2 NLOS環(huán)境下的魯棒策略

如上分析，NLOS會導(dǎo)致位置估計的偏差。NLOS環(huán)境下的代價函數(shù)如下

(25)

從式(25)可以觀察到，NLOS產(chǎn)生的偏置會影響代價函數(shù)的值，最終影響位置信息的估計。相對而言，衛(wèi)星導(dǎo)航信號的多普勒頻率被信號反射影響較小，因此速度信息理論上相對更準，這里基于速度信息構(gòu)建殘差如下

(26)

在NLOS環(huán)境下，殘差會越來越大。這里給出兩種策略。

(1) 魯棒核函數(shù)

這里將Huber魯棒函數(shù)引入FGO算法以抑制NLOS帶來的位置誤差，Huber函數(shù)定位如下[8]

(27)

式中，x為變量；b為閾值。

(2) 自適應(yīng)協(xié)方差矩陣

如式(26)所示，NLOS信號會造成殘差突然變大，自適應(yīng)協(xié)方差矩陣定位如下

(28)

3 實驗與分析

本章利用二種不同的手機采集了靜態(tài)和動態(tài)數(shù)據(jù)，對提出的算法進行驗證。這里采用Google手機定位比賽數(shù)據(jù)集中的一組Google Nexus 5手機靜態(tài)數(shù)據(jù)和一組Xiaomi 8動態(tài)數(shù)據(jù)對算法進行進一步的驗證和分析。

3.1 靜態(tài)數(shù)據(jù)實驗

首先采用Google Nexus 5手機采集的靜態(tài)數(shù)據(jù)對算法進行測試，采集地點為(37.422578°, -122.081678°, -28m)[11]。該場景中，GPS衛(wèi)星2、5、12、15、18、20、21、25、26、29、31被用于定位。圖2所示為跟蹤的衛(wèi)星信號強度隨時間變化，圖3所示為標準LS計算的水平位置和速度誤差。可以看到，水平位置誤差在10m以內(nèi)，速度誤差在0.2m/s以內(nèi)。圖4進一步地給出了水平位置誤差分布、定位精度因子和衛(wèi)星數(shù)量變化。50%概率水平位置誤差為4.8m；可見衛(wèi)星數(shù)很快穩(wěn)定到10顆；HDOP值很快收斂到1以內(nèi)。圖5所示為LS、KF和FGO對應(yīng)的水平位置誤差對比，三種方法速度誤差方差為0.01，位置誤差方差為5。表1所示為以上三種方法對應(yīng)的水平位置誤差統(tǒng)計分析。從統(tǒng)計分析結(jié)果可知，經(jīng)過KF平滑之后的位置誤差均值和均方根降低了19.8% 和19.6%；經(jīng)過FGO算法平滑之后的位置誤差均值和均方根進一步降低了40.1% 和26.3%。圖6給出了水平位置誤差的概率分布，和KF算法相比，50%和90%概率位置誤差減少了48.7% 和28.8%。

圖2 衛(wèi)星信號載噪比Fig.2 Satellite signal C/N0

圖3 水平位置和速度誤差Fig.3 Horizontal position and velocity errors

圖4 水平位置誤差分布、HDOP和衛(wèi)星數(shù)Fig.4 Horizontal position distribution, HDOP and satellite amount

圖5 Nexus 5定位誤差對比Fig.5 Comparison of Nexus 5 horizontal position errors

表1 定位誤差統(tǒng)計分析

這里在量測位置上加上固定偏差30m,以模擬NLOS場景造成的位置誤差，圖7所示為該場景下的位置誤差對比。圖7(a)為模擬從100s開始的10s NLOS場景下的FGO-Huber和自適應(yīng)FGO的定位誤差；圖7(b)和圖7(c)分別為從100s開始的20s和30s NLOS場景下的定位誤差對比。從圖中的位置誤差曲線可以看出，F(xiàn)GO-Huber和自適應(yīng)FGO算法可以有效地消除NLOS信號對水平定位精度的影響。在10s和20s NLOS場景下，F(xiàn)GO-Huber和自適應(yīng)FGO定位誤差類似；但是當NLOS時間變長時，自適應(yīng)FGO算法效果更好，可以有效地減小最大定位誤差。

圖6 水平位置誤差概率分布Fig.6 Probability distribution for horizontal position errors

(a) 10s NLOS定位誤差

3.2 動態(tài)數(shù)據(jù)實驗

進一步采用另一組Google手機定位比賽動態(tài)數(shù)據(jù)評估算法的有效性。此數(shù)據(jù)采用Xiaomi 8手機裝在車上采集，軌跡如圖8所示。位置誤差隨時間變化如圖9所示，F(xiàn)GO算法定位誤差最小。詳細的定位誤差統(tǒng)計分析如表2所示，和LS相比，KF和FGO算法的位置誤差均值減少了16.4%和39.3%，位置誤差均方根誤差也有不同程度的減少。

圖8 動態(tài)實驗軌跡圖Fig.8 Dynamic trajectory

圖9 水平位置誤差對比Fig.9 Comparisons of horizontal position errors

表2 水平定位誤差統(tǒng)計分析

和靜態(tài)實驗類似，在軌跡200s開始增加30m恒定誤差模擬NLOS信號造成的位置偏差，以評估算法在動態(tài)軌跡下的性能。圖10給出了從第200s開始的10s、20s和30s NLOS環(huán)境下的不同算法定位誤差對比。從圖中位置誤差可以看出，LS和KF結(jié)果被NLOS影響較嚴重，尤其是長時間NLOS信號導(dǎo)致LS和KF定位結(jié)果位置誤差異常，最大達40m左右。KF-Huber和自適應(yīng)KF對NLOS造成的誤差有一定的抑制作用，但是當NLOS時間變長時，最大定位誤差仍然在變大，本實驗中最大誤差達到了40m。實驗結(jié)果證明了Huber魯棒核函數(shù)和自適應(yīng)策略對NLOS誤差具有一定的抑制作用。對于10s NLOS場景，F(xiàn)GO-Huber和自適應(yīng)FGO可以有效地抑制NLOS造成的位置誤差異常；但是當NLOS時間變長時，F(xiàn)GO-Huber效果有所下降，在20s和30s NLOS場景下，F(xiàn)GO-Huber算法的最大位置定位誤差大約達到了30m和43m,證明了本文提出的算法對NLOS誤差的抑制作用。

4 結(jié)論

針對城市復(fù)雜環(huán)境下NLOS信號對智能手機定位精度的影響，本文基于因子圖框架和運動模型構(gòu)建位置優(yōu)化方法，并提出了FGO-Huber和自適應(yīng)FGO方法抑制NLOS造成的位置誤差。通過靜態(tài)和動態(tài)實際場景實驗證明了所提算法可以有效地減少NLOS信號造成的位置誤差。根據(jù)實驗結(jié)果可以得出以下結(jié)論：

1)采用速度優(yōu)化可以平滑定位結(jié)果，F(xiàn)GO算法可以有效地提高智能手機的GNSS定位精度；靜態(tài)實驗中，KF平滑之后的位置誤差均值降低了19.8%；經(jīng)過FGO算法平滑之后的位置誤差均值進一步降低了40.1%；動態(tài)實驗中，和LS相比，KF和FGO算法的位置誤差均值減少了16.4%和39.3%。

2)自適應(yīng)協(xié)方差矩陣結(jié)合FGO方法可以有效地克服NLOS信號對智能手機GNSS水平定位精度的影響，提高智能手機GNSS在城市復(fù)雜環(huán)境下的定位精度；動態(tài)實驗中，在10s、20s和30s NLOS環(huán)境下，自適應(yīng)FGO算法可以一直抑制NLOS造成的位置誤差，而FGO-Huber等方法在20s和30s NLOS時長下，位置誤差逐漸變大，最大達到43m。