基于擴展卡爾曼濾波的時鐘模型輔助定位算法*

宋建材,候春萍,薛桂香

(1.天津大學電子信息工程學院,天津 300072;2.河北工業(yè)大學計算機科學與軟件學院,天津 300430; 3.天津商業(yè)大學信息工程學院，天津 300134)

1 引言

全球衛(wèi)星導航系統(tǒng)GNSS(Global Navigation Satellite System)的接收機時鐘精度低，無法與星載原子鐘同步，衛(wèi)星與接收機之間的鐘差Δt是未知的，至少需要4個觀測方程聯(lián)立才能實現(xiàn)用戶三維定位和授時[1]。對于平面運動的車載或船載用戶或有高程輔助的終端用戶，只需要跟蹤接收3顆可見衛(wèi)星就可實現(xiàn)二維定位算法。如果視野內(nèi)可見衛(wèi)星數(shù)目小于3顆時，接收機通常由于缺少足夠的觀測量而無法實現(xiàn)正常定位解算，GNSS系統(tǒng)處于不可用狀態(tài)[2]。隨著我國城鎮(zhèn)化的快速發(fā)展，在城市峽谷和郊區(qū)樹林多的道路上很容易對衛(wèi)星信號有遮擋，引起可見衛(wèi)星數(shù)目不足，使得接收機無法定位的情形愈來愈多，在某些定向干擾場景下，因接收機跟蹤的衛(wèi)星數(shù)量減少而不能正常工作。

為提高在信號遮擋或者某些定向干擾場景時GNSS接收機的可用性，目前常用的手段主要有兩大類：一是利用慣性導航系統(tǒng)INS(Inertial Navigation System)組合[3,4]實現(xiàn)連續(xù)定位，特別是近年隨著微電子機械系統(tǒng)MEMS(Micro Electro-Mechanical Systems)技術的發(fā)展，GNSS/MEMS組合定位方法逐漸成熟[5,6]，增強了GNSS在信號缺失時的可用性，缺點是需陀螺儀與加速度計輔助傳感器，增加了硬件成本；二是從電子地圖精確匹配角度實現(xiàn)了輔助定位[7,8]，但這類算法依賴電子地圖數(shù)據(jù)和地圖匹配算法的精度，定位誤差相對較大，由于與電子地圖的路線匹配，因此僅適用于特定運動路線場景。此外，之前的學者還提出了基于灰色理論的鐘差模型預測方法[9]，但難以利用遞推方法與卡爾曼濾波結(jié)合；時鐘模型最小二乘法解[10]和鐘差系列預測方法[11]的缺點是只利用當前歷元測量信息，定位精度較差。針對上述方法的缺點，本文提出了一種基于接收機時鐘模型的輔助定位算法，主要思想是同時利用跟蹤環(huán)路的偽距和多普勒觀測量，通過擴展卡爾曼濾波EKF(Extend Kalman Filter)實時估算接收機時鐘的鐘偏和鐘漂，構(gòu)建時鐘數(shù)學模型，在導航信號部分缺失情況下采用該時鐘模型參數(shù)進行輔助定位，可以繼續(xù)按照原精度級別完成定位解算。仿真分析和車載實驗結(jié)果表明，該方法可在僅有2～3顆衛(wèi)星信號可見時，實現(xiàn)高精度二維和三維定位，在硬件不變前提下，提高了系統(tǒng)可用性。

2 接收機時鐘模型

在GNSS觀測模型中接收機鐘差對定位精度有非常大的影響，微小的鐘差會導致較大的測距誤差和相位測量誤差。接收機鐘差δt建模為：

δt=δtu+δfu·t(t1≤t≤t2)

其中，δtu：用戶接收機時鐘偏差；δfu：用戶接收機時鐘漂移速度；(t1,t2):鐘差多項式的有效周期。

提取狀態(tài)變量：

用狀態(tài)方程描述為：

接收機時鐘模型可以通過拉氏變換離散化。

其中,L是拉氏變換算子，I是單位矩陣，Ts為測量采樣周期，也是狀態(tài)方程差分步長。接收機時鐘的Kalman濾波狀態(tài)方程可表示為：

xk=Acxk-1+wk-1

其中，wk-1代表噪聲e(t)引起的過程噪聲向量，噪聲向量e(t)所對應的權矩陣為：

其中，St為鐘差噪聲的自協(xié)方差，Sf為頻偏噪聲的自協(xié)方差。

測量噪聲方差：

因為該模型中各個相關的矩陣均為常系數(shù)陣，所以過程噪聲wk的協(xié)方差Qk也不隨時間變化，其值為：

3 擴展卡爾曼濾波

擴展卡爾曼濾波(EKF)是一種非線性系統(tǒng)濾波方法，可將狀態(tài)轉(zhuǎn)移方程和測量方程在當前估值處利用泰勒展式進行線性化。一個非線性系統(tǒng)方程可表示為：

其中：

f表示三者之間的非線性關系(系統(tǒng)狀態(tài)x、輸入量u和時間t)；

h是關于系統(tǒng)狀態(tài)x和時間t的非線性函數(shù)；

w(t)是系統(tǒng)噪聲向量；

v(t)是測量噪聲。

假設在時刻t系統(tǒng)狀態(tài)為x(t)，估計值x*(t)，此時系統(tǒng)和測量函數(shù)分別為：

估計量偏差為：

δx(t)=x(t)-x*(t)

用泰勒級數(shù)展開，略去高階項，得到:

其中：

GNSS的偽距和偽距率觀測方程都是非線性的，因此擴展卡爾曼濾波處理位置速度和時間PVT(Position Velocity and Time)解算，可以降低噪聲干擾，提高接收機定位精度。

4 接收機時鐘輔助定位方程

針對行人、汽車、艦船等低動態(tài)載體而言，接收機運行情況可以用8個狀態(tài)變量來描述，即三個位置分量(x,y,z)、三個速度分量(vx,vy,vz)和接收機時鐘的兩個變量(δtu,δfu)。其狀態(tài)變量為:

(x,y,z,vx,vy,vz,δtu,δfu)

常系數(shù)的狀態(tài)轉(zhuǎn)移矩陣為:

狀態(tài)向量X=[xvx]的協(xié)方差矩陣Qx表示為：

其中Svx為X速度分量的噪聲功率譜密度。

整個算法的流程如圖 1所示。算法首先進行EKF濾波估計變量初始化，然后判斷當前的衛(wèi)星數(shù)目，切換到不同的定位算法。采用多次估計平均方法，可以提高時鐘模型精度，以此作為接收機時鐘參數(shù)，固化存儲在內(nèi)置Flash。

Figure 1 Flowchart of colck-model-assisted positioning algotithm圖1 時鐘輔助定位算法流程圖

5 仿真分析與算法驗證

該實驗利用加拿大NovAtel公司的OEMStar板卡進行數(shù)據(jù)采集，仿真計算機利用ThinkPad X250、Intel i5-5300處理器、8 GB內(nèi)存，操作系統(tǒng)是64位Windows 7，仿真軟件版本是Matlab 2010a。

為了測試接收機跟蹤不同可見衛(wèi)星數(shù)量時，算法自適應切換功能和定位性能，利用OEMstar提供的ASSIGN和UNASSIGN命令指定信道跟蹤特定衛(wèi)星，從而把接收衛(wèi)星信號數(shù)量限定在指定個數(shù)，模擬接收機分別跟蹤大于4顆、4顆、3顆、2顆四種衛(wèi)星數(shù)量的定位場景。接收機狀態(tài)分別有靜態(tài)放置、車載運動兩種。

接收機必須先跟蹤4顆以上衛(wèi)星信號，以便擴展卡爾曼濾波器能夠精確估計出接收機時鐘偏差和漂移速度參數(shù)，得到其數(shù)學模型。1分鐘后通過設置命令減少接收機跟蹤的衛(wèi)星數(shù)量至3顆衛(wèi)星，系統(tǒng)處于信號缺失的不完整星座狀態(tài)，最小二乘法無法三維定位，但通過EKF估計仍能定位，接收機的時鐘偏差和漂移速度曲線如圖2所示。

Figure 2 Receiver clock bias and drift speed curve圖2 接收機鐘偏和鐘漂速度曲線

當接收機靜止狀態(tài)時，EKF定位誤差如圖3所示，包括東向、北向、垂向誤差三個分量。由圖3可知，在信號缺失狀態(tài)下，接收機定位結(jié)果精度穩(wěn)定，大約在1 000 s以內(nèi)定位誤差并沒有出現(xiàn)明顯增大，在1 200 s以后北向誤差逐步增大發(fā)散，大約在1 500 s時北向誤差增大到50 m左右，而東向和垂向誤差變化不明顯；EKF測速誤差如圖4所示，可見信號缺失時卡爾曼濾波的測速誤差仍非常小。

Figure 3 3D position error curve based on EKF during signal outage圖3 信號缺失EKF三維定位誤差曲線

Figure 4 Error curve of 3D velocity measurement during signal outage圖4 信號缺失時EKF三維測速誤差

當接收機處于車載運動狀態(tài)時，定位結(jié)果曲線跟實際工作中的定位曲線對比如圖5示，當衛(wèi)星信號缺失只有3顆可見衛(wèi)星時，利用擴展卡爾曼濾波器估計接收機時鐘參數(shù)輔助，繼續(xù)完成三維定位；當接收機跟蹤的衛(wèi)星信號繼續(xù)減少至2顆衛(wèi)星時，基于擴展卡爾曼濾波的時鐘輔助算法就只能進行2維定位。定位算法可根據(jù)當前的衛(wèi)星數(shù)目自動進行切換。水平定位誤差曲線如圖6所示，在這種情況下，由于擴展卡爾曼濾波對接收機時鐘參數(shù)的精確估計，大約在300 s內(nèi)二維定位誤差也沒有急劇惡化趨勢，在390 s左右系統(tǒng)發(fā)生了解算算法的自動切換，系統(tǒng)的誤差逐漸增大，北向誤差增大到20 m以上，在450 s左右北向誤差增大到30 m以上，不滿足精密定位的要求，但是對于系統(tǒng)的概略位置估計和系統(tǒng)狀態(tài)估計仍有一定的幫助。

Figure 5 Dynamic position curve based on EKF during signal outage圖5 信號缺失時EKF車載動態(tài)定位曲線

Figure 6 2D position error curve based on EKF during signal outage圖6 信號缺失EKF水平定位誤差曲線

6 結(jié)束語

本文針對不完整星座提出了一種新的基于EKF的時鐘模型輔助定位方法。首先在星座完整時，通過綜合利用偽距和多普勒觀測量，通過擴展卡爾曼濾波器實時估算得到接收機鐘偏和鐘漂參數(shù)，通過構(gòu)建接收機時鐘數(shù)學模型，在導航信號缺失情況下，利用擴展卡爾曼濾波技術根據(jù)該時鐘模型進行推導輔助定位。通過分別對靜態(tài)和車載實驗分析，結(jié)果表明該方法可在衛(wèi)星導航信號缺失、星座不完整時仍能夠在一定時間內(nèi)以較高精度繼續(xù)完成實時定位解算，在不增加硬件成本前提下提高了可用性，具有較好的應用價值。

參考文獻:

[1] Xie Gang.Principles of GPS and receiver design[M].Beijing:Publishing House of Electronics Industry,2009.(in Chinese)

[2] Elliott K,Christopher H.Understanding GPS:Principles and applications [M].Boston:Artech House,2005.

[3] Miller I,Campbell M.Sensitivity analysis of a tightly-coupled GPS/INS system for autonomous navigation [J].IEEE Transactions on Aerospace and Electronic Systems,2012,48(2):1115-1135.

[4] Jwo D J,Yang C F,Chuang C H,et al.A novel design for the ultra-tightly coupled GPS/INS navigation system[J].Journal of Navigation,2012,65(4):717-747.

[5] Qin Hong-lei,Cong Li,Sun Xing-li.Accuracy improvement of GPS/MEMS-INS integrated navigation system during GPS signal outage for land vehicle navigation [J].Journal of Systems Engineering and Electronics,2012,23(2):256-264.

[6] Angrisano A,Petovello M,Pugliano G.Benefits of combined GPS/GLONASS with low-Cost MEMS IMUs for vehicular urban navigation [J].Sensors,2012,12(4):5134-5158.

[7] Obst M,Bauer S,Reisdorf P,et al.Multipath detection with 3D digital maps for robust multi-constellation GNSS/INS vehicle localization in urban areas[C]∥Proc of 2012 IEEE Intelligent Vehicles Symposium (IV),2012:184-190.

[8] Fouque C,Bonnifait P.Matching raw GPS measurements on a navigable map without computing a global position [J].IEEE Transactions on Intelligent Transportation Systems,2012,13(2):887-898.

[9] Teng Yun-long,Shi Yi-bing,Zheng Zhi.Research on positioning method under GPS incomplete constellation[J].Journal of Astronautics,2011,32(1):93-97.(in Chinese)

[10] Wang Er-shen, Zhang Shu-fang,Hu Qing.Incomplete con-

stellation positioning method for GPS receiver augmented with clock bias model [J].Journal of Dalian Maritime University,2009,35(2):59-61.(in Chinese)

[11] Guo Rong-chang, Jin Wen-jun,Fang Duo-wang,et al.Reseach on positioning method under GPS incomplete constellation[J].Journal of the China Railway Society,2015,37(3):42-45.(in Chinese)

附中文參考文獻：

[1] 謝剛.GPS原理與接收機設計[M].北京:電子工業(yè)出版社,2009.

[9] 滕云龍,師奕兵,鄭植.GPS不完整星座定位方法研究[J].宇航學報,2011,32(1):93-97.

[10] 王爾申,張淑芳,胡青.基于時鐘偏差模型的GPS不完整星座定位方法[J].大連海事大學學報,2009,35(2):59-61.

[11] 郭榮昌,靳文軍,范多旺,等.GPS不完備條件下定位方法研究[J].鐵道學報,2015,37(3):42-45.