基于無跡卡爾曼濾波的全球導航衛星系統精密定位算法研究

周少波，魏 征，李燈熬

(1.太原理工大學 信息與計算機學院，山西 晉中 030600；2.陸軍航空兵學院 政治工作部人力資源處，北京 101123)

導航的“最后一公里”問題是近年來導航領域的研究熱點。其中關于行人室內定位的問題研究已經很多，也有了一些比較成熟的技術；而行人室外特別是城市街道中的行人定位導航研究目前相對較少，行人室外導航技術不僅可以應用于智能交通和無人駕駛領域，而且可以為城市行人提供個性化導航服務，比如逛街想避開犯罪率高或者擁擠的地段，或者臨時改變目的地，要快速找到某個商場或者健身中心等。微軟在2012年就申請了一種GNSS行人導航技術的專利[1]。要想高質量、高精度地實現行人導航服務，對行人的精密定位是首先要解決的問題。目前，利用接收機實現定位的方法主要按照是否與距離有關來進行劃分。其中與距離有關的相關算法主要是通過接收信號強度、到達時間，到達時差，以及到達角度等方式來完成定位計算。與距離無關的算法主要有：指紋定位、近似點定位等。文獻[2]提出了一種基于卡爾曼濾波的RSSI定位算法，獲得了比較高的穩定度和精度；文獻[3]針對巷道信號有延時的特點，提出能夠通過將基于時間差的定位方式與指紋定位方式相結合，從而有效的提高定位精度。文獻[4]則是提出了一種基于卡爾曼濾波的TDOA/TOA混合定位算法；這些算法都容易受到障礙物等環境影響。而城市街道環境具有典型的環境復雜性和人群的密集性及流動性，傳統的行人定位算法并不適用于對城市街道行人進行精密定位[5]。

智能手機和可穿戴式設備等移動智能終端已經深入到人們的日常生活中。可以將這些移動終端的衛星定位模塊以信息融合的理念應用于GNSS精密定位[6]。具體的說，根據城市街道人群密集的特點和定位數據誤差的概率分布特性，先將周邊行人和目標行人的衛星定位數據通過基站等無線通信手段上傳到后臺服務器；后臺服務器利用周邊行人定位數據通過卡爾曼濾波對目標行人定位數據進行修正，達到精密定位的目的；然后將處理結果通過無線通信手段下發到目標行人移動智能終端，實現了定位數據共享。這樣不僅將城市街道人群密集這個傳統定位算法看來是劣勢的問題變成了可以共享定位信息的優勢，而且也比較好的規避掉了城市街道障礙物多和電磁環境復雜這個問題。

1 基于SVM的LOS信號分類算法

在復雜的城市環境下，由于GNSS信號的遮擋導致部分衛星發射的信號以非視距傳播，嚴重影響偽距與載波相位的測量，因此將其產生的偏差量ρNLOS引入偽距方程與載波相位方程中。假設第i顆衛星的信號以NLOS傳播，其偽距方程與載波相位方程分別為：

(1)

(2)

(3)

(4)

(5)

為了獲得一組不包含非視距偏差的偽距方程，采用支持向量機(SVM)分類算法將LOS信號與NLOS信號進行分類。

如果在信號樣本原始的空間中無法將需要的信號進行有效的分離，那么可以利用SVM將其映射到高維的空間，并求出最優超平面從而達到劃分的目的。本文利用SVM進行分類的特征為：載噪比、均方根誤差、衛星仰角，然后根據分類的特征對樣本進行訓練。由于本文進行的分類為非線性可分，其分類超平面如式(6)：

(6)

其決策函數如式(7)

(7)

當f(x)>0時，樣本屬于LOS路徑信號，否則屬于NLOS路徑信號。其中，核函數K(xi,xj)采用徑向基(RBF)核函數，如下式(8)

(8)

式中，σ為超參數，即核寬度。σ值的確定根據最小均方誤差為依據，通過對樣本進行訓練和測試獲得，由多次實驗得到最優的σ值為1.050 732[7].

通過SVM分類算法即可得到一組較為理想的不包含NLOS偏差ρNLOS的直達信號，其偽距方程如式(5)所示，方程中的Nj通過LAMBDA算法來進行快速準確的求解，并將其移到方程右邊，得到偽距校正方程如式(9)：

(9)

其中，(x,y,z)為用戶的三維空間坐標，(xj,yj,zj)為所發射衛星的坐標。

2 無跡卡爾曼濾波算法

實現精密定位的核心算法是UKF濾波算法，它是一種由經典卡爾曼濾波發展而來的最優化方法[8]。

2.1 無跡變換

UKF的核心是無跡變化，其特點是根據原始狀態的空間分布進行有效的采樣，從而有效的確保采樣點的各種特征值，特別是均值與協方差與原狀態保持一致[9]。然后將采集到的采樣點通過非線性函數得到所需的函數值組成的點集中，并對其進行一系列的特征值計算。該方法不是對函數進行泰勒展開，并忽略高階項，而是利用統計特性實現函數值的計算，能夠有效的避免傳統的擴展卡爾曼濾波中引起的截斷誤差[10]。因此利用無跡卡爾曼可以有效地提高接收機的定位精度；

無跡變換算法流程主要包括兩步[11]：

1) 設狀態向量的維數為n，計算2n+1個采樣點，其表達式為:

(10)

2) 計算不同采樣點的權值：

(11)

其中，參數λ為縮放比列參數，主要的作用是減小預測誤差，λ的計算公式為λ=α2(n+κ)-n.

2.2 無跡卡爾曼濾波

UKF與卡爾曼濾波相同點是同為最小方差估計，不同點是在利用卡爾曼濾波模型時利用預測均方誤差以及先驗信息來計算關系矩陣，從而實現迭代與遞推。而UKF則是利用觀測量的協方差陣以及估計量來構建關系矩陣[12]。UKF的具體算法流程為：

1) 利用無跡變換得到一組個數為2n+1的帶有權值的有效的采樣點，即sigma點

(12)

2) 分別計算每個采樣點的預測值。

(13)

3) 計算所求狀態向量k+1時刻的狀態值及對應的協方差矩陣。

(14)

(15)

4) 利用無跡變換并根據k+1時刻的狀態值生成新的sigma點

(16)

5) 將經上一步無跡變換產生的sigma點代入非線性方程中，得到預測值

(17)

6) 對通過加權求和得到系統預測的均值及協方差。

(18)

(19)

(20)

7) 計算卡爾曼濾波增益

(21)

8) 計算系統的狀態量更新和協方差更新

(22)

(23)

3 數學模型的構建

3.1 狀態方程的建立

假設行人在街道上行走，采樣時間間隔為T，根據動力學方程，

(24)

式中：S代表距離坐標，k表示歷元時刻，T表示兩個歷元之間的時間間隔，v表示速度，a表示加速度。

結合城市街道行人實際情況，將經度和緯度方向的位置坐標和速度作為狀態量，則狀態向量可以表示為:

X=[xvxyvy]T.

(25)

一步轉移陣可以根據動力學方程建立為：

(26)

噪聲驅動陣為：

(27)

最后，建立系統狀態方程為：

(28)

很明顯，系統方程是線性的。

3.2 觀測方程的建立

根據街道行人密集，距離近，流動性大的特點。先計算出被定位目標行人與周圍行人的距離，然后選取距離最近的n個作為觀測量，那么把距離作為觀測量的情況下，觀測方程是：

(29)

式中，m表示所選距離最近里的第m個觀測行人，卡爾曼濾波在時間更新一次以后，量測更新迭代次數為n次；顯然，觀測方程非線性，所以對卡爾曼濾波的量測更新不能使用經典卡爾曼濾波，本文采用無跡卡爾曼濾波算法。

4 Matlab實驗仿真及分析

仿真實驗使用MatlabR2016a仿真軟件進行，對行人的真實行走軌跡和衛星定位系統的原始定位數值以及經過優化算法濾波的軌跡做出對比，然后對SVM分類算法前后以及濾波前濾波后的誤差進行對比，并配有相應的仿真圖像，形象定量的顯示出算法的有效性。

4.1 實驗仿真及其分析

首先設置采樣時間間隔T=1 s，迭代次數即運行時間N=80；選取距離最近的周邊行人個數n=10.

從圖1中可以看出行人真實移動軌跡比較平直，吻合了城市街道特點；衛星定位系統的直接觀測定位抖動比較大，但是總趨勢跟真實軌跡一致，這是因為定位數據存在誤差的原因；濾波后的定位軌跡基本貼近真實行走軌跡，雖然也有抖動，但是幅度明顯變小，說明誤差變小，精度提高了。證明算法是有效的。

圖1 行人真實軌跡和衛星定位以及濾后波軌跡對比Fig.1 Comparison of pedestrian real trajectory and satellite positioning and UKF filter trajectory

為了體現SVM算法的抗粗差性能，將用戶接收機定位過程中的第100歷元時刻人為在GNSS觀測中加入+200 m的粗差，之后再利用本文所設計的算法方法進行仿真。

圖2為沒有對GNSS信號進行SVM分類算法的3個方向定位誤差仿真結果。圖3為利用SVM對非視距信號進行分類的仿真結果。結果表明加入粗差后，通過系統會產生很大的誤差，但應用SVM分類算法受粗差的影響較小，因此，本文提出的算法具有很好的抗粗差性能。

圖2 GNSS系統抗粗差性能Fig.2 Robustness of the GNSS system

圖3 SVM分類算法抗粗差性能Fig.3 Robustness of the fusion systems in the B region

圖4 濾波前后誤差對比Fig.4 Comparison of errors before and after UKF

圖4中圖形點表示衛星定位與行人真實位置的均方根誤差，方形點表示算法濾波后的定位結果與行人真實位置的均方根誤差，從圖中可以看出，濾波后的定位誤差明顯小于衛星原始定位誤差，也證明了算法的有效性。

5 結束語

首先考慮城市街道障礙物多，行人流動性大，電磁環境復雜的情況，創新性利用SVM分類以及UKF融合定位信息理念和衛星定位的特點，通過后臺處理數據，規避了這些不利因素；通過在偽距方程及載波相位仿真中引入非視距誤差，并利用SVM分類算法實現視距信號及非視距信號的分類，有效的提高了接收到的衛星信號的質量。其次在數學建模時，根據城市街道平坦的特點，不考慮高程方向的信息，減少狀態量維度和計算量，提高了實時性，而且根據觀測方程不符合線性系統的特點，使用無跡卡爾曼濾波對其進行處理，解決了經典卡爾曼濾波只適用于線性系統的問題，精度也保持得比較高。仿真結果標明，濾波算法能夠有效提高城市行人定位精度，比較好地解決了這個行人導航技術的關鍵問題。