基于無跡卡爾曼濾波的全球導航衛(wèi)星系統(tǒng)精密定位算法研究

周少波，魏 征，李燈熬

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

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

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

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

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

(1)

(2)

(3)

(4)

(5)

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

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

(6)

其決策函數(shù)如式(7)

(7)

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

(8)

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

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

(9)

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

2 無跡卡爾曼濾波算法

實現(xiàn)精密定位的核心算法是UKF濾波算法，它是一種由經(jīng)典卡爾曼濾波發(fā)展而來的最優(yōu)化方法[8]。

2.1 無跡變換

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

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

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

(10)

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

(11)

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

2.2 無跡卡爾曼濾波

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

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

(12)

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

(13)

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

(14)

(15)

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

(16)

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

(17)

6) 對通過加權求和得到系統(tǒng)預測的均值及協(xié)方差。

(18)

(19)

(20)

7) 計算卡爾曼濾波增益

(21)

8) 計算系統(tǒng)的狀態(tài)量更新和協(xié)方差更新

(22)

(23)

3 數(shù)學模型的構建

3.1 狀態(tài)方程的建立

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

(24)

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

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

X=[xvxyvy]T.

(25)

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

(26)

噪聲驅動陣為：

(27)

最后，建立系統(tǒng)狀態(tài)方程為：

(28)

很明顯，系統(tǒng)方程是線性的。

3.2 觀測方程的建立

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

(29)

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

4 Matlab實驗仿真及分析

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

4.1 實驗仿真及其分析

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

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

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

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

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

圖2 GNSS系統(tǒng)抗粗差性能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中圖形點表示衛(wèi)星定位與行人真實位置的均方根誤差，方形點表示算法濾波后的定位結果與行人真實位置的均方根誤差，從圖中可以看出，濾波后的定位誤差明顯小于衛(wèi)星原始定位誤差，也證明了算法的有效性。

5 結束語

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