李明玉，李奇奮，艾 倫，李 斌

車載環(huán)境下GPS矢量跟蹤主濾波器的研究實(shí)現(xiàn)*

李明玉，李奇奮，艾 倫，李 斌

（北京遙測(cè)技術(shù)研究所 北京 100076）

以C語(yǔ)言軟件接收機(jī)為平臺(tái)，設(shè)計(jì)實(shí)現(xiàn)GPS導(dǎo)航接收機(jī)矢量跟蹤算法主濾波器模型。利用標(biāo)量測(cè)量依次處理法對(duì)矢量跟蹤導(dǎo)航濾波器進(jìn)行改造，解決矢量跟蹤算法的計(jì)算量大和單個(gè)通道故障會(huì)對(duì)全局造成影響這兩個(gè)缺點(diǎn)，提高系統(tǒng)的計(jì)算效率和容錯(cuò)能力。分析標(biāo)量測(cè)量依次處理方法計(jì)算量小的優(yōu)勢(shì)，并進(jìn)行跑車試驗(yàn)。試驗(yàn)結(jié)果表明，標(biāo)量測(cè)量依次處理法減小了矢量跟蹤主濾波器的運(yùn)算量，同時(shí)提高了信號(hào)故障檢測(cè)能力。相比于單點(diǎn)定位，導(dǎo)航結(jié)果具有更高的速度測(cè)量精度和更加平滑的導(dǎo)航定位誤差。

矢量跟蹤；卡爾曼濾波器；標(biāo)量測(cè)量依次處理

引 言

矢量跟蹤是一種革命性的技術(shù)，它改變了傳統(tǒng)衛(wèi)星導(dǎo)航技術(shù)中將信號(hào)處理與信息處理割裂的方式，通過(guò)信號(hào)與信息的融合，拓展了衛(wèi)星導(dǎo)航技術(shù)的適用性。這種方法不僅可以增加系統(tǒng)的抗干擾性能，還可以降低系統(tǒng)噪聲帶寬，提高跟蹤穩(wěn)定性。主濾波器利用擴(kuò)展卡爾曼濾波器EKF（Extended Kalman Filter）完成定位，充分利用了車載系統(tǒng)的慣性原理。通過(guò)前后歷元運(yùn)動(dòng)狀態(tài)的遞推關(guān)系，可以得到更加符合運(yùn)動(dòng)規(guī)律的定位結(jié)果[1]。在可見衛(wèi)星數(shù)不足無(wú)法完成單點(diǎn)定位時(shí)，主濾波器仍然可以獲得一段時(shí)間內(nèi)相對(duì)準(zhǔn)確的定位結(jié)果，具有更強(qiáng)的抗干擾能力。

計(jì)算量大是限制矢量跟蹤應(yīng)用的主要問(wèn)題之一[2,3]。由于卡爾曼濾波常規(guī)算法的運(yùn)算量與狀態(tài)維數(shù)的三次方成正比，尤其是量測(cè)更新時(shí)的矩陣求逆，包含了大量的乘法運(yùn)算,使得高維狀態(tài)模型的濾波對(duì)導(dǎo)航解算處理器產(chǎn)生了沉重的負(fù)擔(dān)[4]。另外，矢量跟蹤技術(shù)將所有通道的衛(wèi)星信息輸入到一個(gè)卡爾曼濾波器中進(jìn)行處理，如果一個(gè)通道的信號(hào)出現(xiàn)異常，可能會(huì)對(duì)整個(gè)系統(tǒng)的定位結(jié)果產(chǎn)生影響[1]，因此對(duì)導(dǎo)航濾波器觀測(cè)量的故障檢測(cè)是必要的。

1 矢量跟蹤導(dǎo)航濾波模型

矢量跟蹤的概念最早是由Spilker于二十世紀(jì)八十年代提出的[4]，其基本思想是建立定位信息與信號(hào)跟蹤信息的聯(lián)系，利用所有通道數(shù)據(jù)完成導(dǎo)航解算，再由已知的導(dǎo)航結(jié)果預(yù)測(cè)信號(hào)跟蹤參數(shù)。矢量跟蹤方法有效利用接收機(jī)各通道之間的固有聯(lián)系，挖掘信號(hào)跟蹤與導(dǎo)航解算的內(nèi)在聯(lián)系，從而顯著增強(qiáng)導(dǎo)航系統(tǒng)的穩(wěn)定性和連續(xù)性。

矢量跟蹤的導(dǎo)航濾波器用于估計(jì)導(dǎo)航接收機(jī)的位置、速度和時(shí)間狀態(tài)，并為偽碼和載波的閉環(huán)控制量提供基礎(chǔ)信息。主濾波器的狀態(tài)方程設(shè)置為三軸位置偏差、三軸速度偏差，以及系統(tǒng)鐘差、鐘漂，共八個(gè)維度，如式（1）所示。

其中，

測(cè)量信息為矢量跟蹤各通道預(yù)處理濾波器得到的偽距偏差與偽距率偏差。如式（4）所示

狀態(tài)矢量和觀測(cè)矢量間的關(guān)系由泰勒展開進(jìn)行線性化，得

其中，

下標(biāo)表示衛(wèi)星，表示用戶。為接收機(jī)到導(dǎo)航衛(wèi)星的距離。

系統(tǒng)狀態(tài)矢量由測(cè)量更新量和由系統(tǒng)動(dòng)態(tài)模型預(yù)測(cè)的下一歷元預(yù)測(cè)值進(jìn)行更新。得到的誤差狀態(tài)用于修正下一歷元的導(dǎo)航結(jié)果。

矢量跟蹤主濾波器的卡爾曼濾波預(yù)測(cè)過(guò)程如下

2 標(biāo)量測(cè)量依次處理方法

導(dǎo)航產(chǎn)品要求定位解算的實(shí)時(shí)性，而矢量跟蹤導(dǎo)航濾波器在更新過(guò)程中，式（12）中的矩陣求逆運(yùn)算的計(jì)算量非常大。為避免矩陣求逆的運(yùn)算，可以采用標(biāo)量測(cè)量依次處理方法進(jìn)行解決。

將式（6）中的測(cè)量矩陣記為

然后對(duì)當(dāng)前時(shí)刻獲得的個(gè)觀測(cè)量，按照式（16）~式（18）逐個(gè)進(jìn)行狀態(tài)更新和均方誤差矩陣校正。

理論上，只要參與計(jì)算的觀測(cè)量y與其噪聲方差r和測(cè)量向量一一對(duì)應(yīng)，則最終計(jì)算的結(jié)果與觀測(cè)量之間的計(jì)算次序無(wú)關(guān)。由于等式(17)右邊的分母是一個(gè)標(biāo)量，整個(gè)計(jì)算過(guò)程不再涉及矩陣的求逆運(yùn)算，可以減小卡爾曼濾波器的運(yùn)算消耗。

將式（18）的右邊記為b，即

這一方法帶來(lái)的優(yōu)勢(shì)在于，當(dāng)某一觀測(cè)量出現(xiàn)異常時(shí)，可以在進(jìn)行判定后直接處理下一個(gè)觀測(cè)量。傳統(tǒng)的矩陣運(yùn)算也可以利用類似的方法進(jìn)行完好性監(jiān)測(cè)，但是通常需要在完成矩陣計(jì)算后，才能進(jìn)行判定，此時(shí)再去掉異常測(cè)量的過(guò)程較為復(fù)雜。利用標(biāo)量測(cè)量依次處理法則使得這一過(guò)程變得簡(jiǎn)單高效。

當(dāng)矢量跟蹤過(guò)程中，突然丟失衛(wèi)星的跟蹤信號(hào)時(shí)，相應(yīng)通道的觀測(cè)量將出現(xiàn)異常。通過(guò)對(duì)標(biāo)量測(cè)量依次處理法這一優(yōu)勢(shì)的利用，可以快速地檢測(cè)出異常信號(hào)，并進(jìn)行剔除以避免污染全局濾波結(jié)果。

3 試驗(yàn)測(cè)試

3.1 場(chǎng)景設(shè)置

試驗(yàn)利用車載數(shù)據(jù)采集系統(tǒng)，對(duì)真實(shí)GPS信號(hào)進(jìn)行采集以進(jìn)行性能驗(yàn)證。試驗(yàn)場(chǎng)景選取了北京G7京新高速公路上的一段路線，路線中有部分指示牌遮擋信號(hào)。試驗(yàn)車向西北方向行駛，由A點(diǎn)至B點(diǎn)，在圖中標(biāo)出。起點(diǎn)為北緯40.08557°、東經(jīng)116.27343°、高度36.8176m；終點(diǎn)為北緯40.09121°、東經(jīng)116.26396°、高度31.8911m。行駛速度由15m/s左右加速至25m/s左右。定位結(jié)果輸出頻率10Hz，總時(shí)長(zhǎng)49.9s。場(chǎng)景選擇如圖1所示。

圖1 試驗(yàn)場(chǎng)景及路線

試驗(yàn)中使用的數(shù)據(jù)中頻為15.42MHz，采樣率為62.5MHz。軟件接收機(jī)分別使用標(biāo)量跟蹤和矢量跟蹤兩種模式進(jìn)行信號(hào)處理，環(huán)路積分時(shí)間為1ms。

3.2 定位結(jié)果

本文對(duì)矢量跟蹤算法與傳統(tǒng)單點(diǎn)定位算法的定位結(jié)果進(jìn)行對(duì)比。圖2分別給出了矢量跟蹤與標(biāo)量跟蹤在ECEF坐標(biāo)系下的三軸定位和測(cè)速誤差。

從圖2左側(cè)的定位誤差可以看到，使用矢量跟蹤主卡爾曼濾波器，系統(tǒng)具有更加平滑的定位噪聲，可以得到更加符合運(yùn)動(dòng)規(guī)律的定位結(jié)果。右側(cè)的測(cè)速誤差顯示，矢量跟蹤的速度誤差精度更高。

利用矢量跟蹤主濾波器解算得到的三軸定位和測(cè)速的誤差的均方差與標(biāo)量跟蹤單點(diǎn)定位的誤差對(duì)比如表1所示。

從表1可以看出，矢量跟蹤輸出的三軸速度誤差比單點(diǎn)定位的速度誤差提高約50%。三軸定位結(jié)果與傳統(tǒng)標(biāo)量跟蹤定位精度相當(dāng)，具有很高的定位精度，同時(shí)定位誤差更加平滑。

3.3 可見衛(wèi)星不足時(shí)的定位

單點(diǎn)定位時(shí)至少需要4顆及以上的導(dǎo)航衛(wèi)星可見，才能夠進(jìn)行正常解算。但是在復(fù)雜觀測(cè)環(huán)境下，如城市峽谷中，GNSS信號(hào)經(jīng)常會(huì)受到建筑物等遮擋，出現(xiàn)可見衛(wèi)星數(shù)不足4顆的情況，例如本文的跑車試驗(yàn)中，由于試驗(yàn)車受到了高架橋的遮擋，在約2s的時(shí)間里，出現(xiàn)了可見衛(wèi)星數(shù)出現(xiàn)不足4顆的情況，此時(shí)傳統(tǒng)接收機(jī)所使用的單點(diǎn)定位方法已經(jīng)無(wú)法給出定位解，如圖3所示。圓點(diǎn)代表單點(diǎn)定位結(jié)果在北-東坐標(biāo)平面上的軌跡。試驗(yàn)車并沒有產(chǎn)生過(guò)跳躍式的運(yùn)動(dòng)，圖3中兩個(gè)圓點(diǎn)之間的跳躍式變化是因?yàn)閱吸c(diǎn)定位在信號(hào)遮擋時(shí)已經(jīng)無(wú)法給出定位解，所以不能顯示。

表1 矢量跟蹤與標(biāo)量跟蹤定位測(cè)速誤差均方差

基于EKF的矢量跟蹤導(dǎo)航濾波器，則可以通過(guò)利用載體位置速度信息的相關(guān)性，對(duì)載體未來(lái)的運(yùn)動(dòng)狀態(tài)進(jìn)行預(yù)測(cè)。即使在觀測(cè)量較少的情況下，矢量跟蹤仍然給出了相對(duì)準(zhǔn)確的定位信息。相同條件下，基于EKF的定位算法給出的定位結(jié)果如圖4所示。圖4中可以看出，導(dǎo)航主濾波器給出的定位結(jié)果完整連續(xù)，符合載體運(yùn)動(dòng)特征。

圖3 單點(diǎn)定位結(jié)果

圖4 矢量跟蹤定位結(jié)果

當(dāng)原本可以正常跟蹤的衛(wèi)星信號(hào)，由于受到遮擋而中斷時(shí)，該信號(hào)通道送入主濾波器的觀測(cè)量就會(huì)出現(xiàn)異常。傳統(tǒng)的卡爾曼濾波算法，只有當(dāng)進(jìn)行完測(cè)量更新后，才能發(fā)現(xiàn)某個(gè)觀測(cè)量異常，這時(shí)只能首先刪除掉測(cè)量矩陣和觀測(cè)噪聲協(xié)方差陣中對(duì)應(yīng)的行和列元素，然后重復(fù)計(jì)算過(guò)程。這將增加很多額外的計(jì)算量，影響系統(tǒng)效率。

而利用標(biāo)量測(cè)量依次處理法對(duì)于異常觀測(cè)量的處理則非常的方便和高效。在進(jìn)行卡爾曼濾波器校正過(guò)程之前，首先對(duì)觀測(cè)向量的元素進(jìn)行監(jiān)測(cè)，判斷觀測(cè)量元素的可用性，有效的解決了不必要的重復(fù)計(jì)算問(wèn)題。同時(shí)，該方法也方便對(duì)卡爾曼濾波器狀態(tài)進(jìn)行監(jiān)測(cè)，當(dāng)連續(xù)多個(gè)觀測(cè)量在多個(gè)時(shí)刻被檢測(cè)出異常，則需要對(duì)故障進(jìn)行處理或重置濾波器。

4 總結(jié)

本文對(duì)矢量跟蹤中主濾波器進(jìn)行了設(shè)計(jì)和實(shí)現(xiàn)，利用標(biāo)量測(cè)量依次處理方法用于主濾波器計(jì)算過(guò)程中的效率優(yōu)化和自主完好性監(jiān)測(cè)，并利用車載數(shù)據(jù)采集平臺(tái)采集到的真實(shí)GPS導(dǎo)航數(shù)據(jù)進(jìn)行了性能測(cè)試。通過(guò)與單點(diǎn)定位進(jìn)行對(duì)比，矢量跟蹤導(dǎo)航濾波器在定位測(cè)速精度和持續(xù)定位能力方面具有良好的性能。結(jié)果表明，利用標(biāo)量測(cè)量依次處理法，提高了系統(tǒng)的實(shí)時(shí)性和容錯(cuò)能力，快速去除了異常觀測(cè)量信息。算法提高了導(dǎo)航系統(tǒng)的速度測(cè)量精度。在可見衛(wèi)星數(shù)少于4顆，輸出準(zhǔn)確的定位結(jié)果，保持定位連續(xù)性，具有較強(qiáng)的持續(xù)定位能力。

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

[2] SCHMIDT G T. INS/GPS technology trends[J]. Ins/gps Technology Trends, 2010.

[3] 程俊仁, 劉光斌, 姚志成. GNSS接收機(jī)矢量跟蹤算法研究綜述[J]. 宇航學(xué)報(bào), 2014, 35(4):380–387.

[4] SPILKER J J, CALIF W. Vector delay lock loop processing of radiolocation transmitter[P]. United States Patent: 5398034, 1995-03-14.

[5] BROWN R G, HWANG P Y C. Introduction to random signals and applied kalman filtering[M].3rd edition, Wiley, 1996.

[6] ELLIOTT D K. GPS原理與應(yīng)用[M]. 邱致和,王萬(wàn)義,譯. 北京: 電子工業(yè)出版社, 2002.

Research and implementation on GNSS vector tracking kalman filter in vehicle environment

LI Mingyu, LI Qifen, AI Lun, LI Bin

（Beijing Research Institute of Telemetry, Beijing 100094, China）

This paper designs and implements the main filter model of GPS navigation receiver vector tracking algorithm based on C language software receiver. The vector tracking navigation filter is modified by the scalar measurement sequential processing method, which solves the two shortcomings of the vector tracking algorithm, including the huge computational complexity and the single channel failure will affect the overall situation, and improve the system's computational efficiency and fault tolerance. The computational advantage of the method was analyzed and a sports car experiment was carried out. The results show that the scalar measurement sequential processing method greatly reduces the computational complexity of the vector tracking main filter and improves the signal fault detection capability. Compared to single point positioning, the navigation results have higher speed measurement accuracy and smoother navigation positioning error.

Vetor tracking; Extended kalman filter; Scalar measurement processing method

TN996

A

CN11-1780(2019)04-0049-05

李明玉 1993年生，碩士生在讀，主要從事衛(wèi)星導(dǎo)航方向研究。

李奇奮 1985年生，工程師，主要從事衛(wèi)星導(dǎo)航方向研究。

艾倫 1982年生，高級(jí)工程師，主要從事衛(wèi)星導(dǎo)航方向研究。

李斌 1969年生，研究員，碩士生導(dǎo)師，主要從事衛(wèi)星導(dǎo)航方向研究。

Email:ycyk704@163.com TEL:010-68382327 010-68382557

國(guó)家自然科學(xué)基金；教育部新世紀(jì)人才支持計(jì)劃

2018-07-19

2019-05-24