自適應漸消Kalman濾波算法在RTK中的應用研究

高亞豪，左啟耀，鄒志勤，李 峰

(北京自動化控制設備研究所，北京 100074)

0 引言

基于載波相位的動態(tài)差分(Real-Time Kinematic，RTK)技術(shù)中，LAMBDA算法是求解整周模糊度最常用的算法之一。它分為模糊度實數(shù)估計和模糊度整數(shù)搜索2個過程。模糊度的實數(shù)估計為模糊度參數(shù)提供一個搜索初始值，通常情況下是浮點數(shù)。高精度的模糊度浮點解能夠減小后續(xù)的搜索空間，有助于提高模糊度固定的成功率[4]。

實際中一般采用普通的最小二乘法求解模糊度浮點解，但它很難實現(xiàn)單歷元解算，通常需要利用多歷元的觀測量組建法方程。當用于組建法方程的歷元數(shù)增加時，會增加未知的流動站天線位置參數(shù)，使得矩陣維數(shù)增大，計算量增加[15]。若各歷元間的時間間隔過小，又會帶來方程病態(tài)的問題。此外，由于流動站處于不斷運動中，觀測環(huán)境復雜多變，會經(jīng)常出現(xiàn)信號失鎖、周跳等現(xiàn)象，這時就需要進行周跳修復或重新解算整周模糊度，實現(xiàn)過程變得復雜。

針對這一情況，本文提出了一種應用于RTK技術(shù)中的Kalman濾波算法。該算法將模糊度參數(shù)作為狀態(tài)向量的一部分，在整個定位過程中實時估計每個觀測時刻的模糊度浮點解，起到了周跳修復的作用。此外，引入自適應漸消的濾波方法，使濾波器能夠?qū)崟r地跟蹤動態(tài)的變化。為了準確估計信息協(xié)方差矩陣，利用滑動窗的方法收集新息，克服了衛(wèi)星變化、參考星改變等情況造成的自適應調(diào)整中斷，且保證了濾波結(jié)果的平滑。

1 RTK中的Kalman濾波模型

在整周模糊度浮點解求解的過程中，系統(tǒng)的狀態(tài)向量中除了包含接收機天線的位置和速度參數(shù)外，將整周模糊度加入到狀態(tài)向量中，通過偽距、載波相位雙差觀測值進行濾波估計，得到接收機天線的位置、速度以及模糊度浮點解。

設待估計的狀態(tài)向量x為

x=(r,v,N1,N2)T

(1)

該濾波器的動態(tài)模型采用等速模型，則對應接收機位置和速度參數(shù)的狀態(tài)轉(zhuǎn)移函數(shù)是一個線性函數(shù)，用矩陣的形式可表示為

(2)

其中，I為3×3的單位陣，ts為2個觀測歷元之間的時間間隔。

對于模糊度參數(shù)，當衛(wèi)星信號跟蹤正常時，其對應的模糊度固定不變；當發(fā)生周跳或失鎖時，為了保證對應的模糊度能夠快速收斂，需要對這些模糊度參數(shù)重新賦值，并更新協(xié)方差矩陣中的相應元素。因此，這一狀態(tài)轉(zhuǎn)移過程是非線性的。

系統(tǒng)的過程噪聲協(xié)方差矩陣可設為對角陣，其對角線元素的大小根據(jù)實際載體的運動狀態(tài)確定。

設系統(tǒng)的觀測向量y為

y=(φ1,φ2,p1,p2)T

(3)

根據(jù)雙差載波相位和偽距觀測方程，可得觀測函數(shù)為

(4)

其對應的觀測噪聲協(xié)方差矩陣R為

(5)

2 自適應漸消Kalman濾波算法

為了避免濾波發(fā)散，自適應漸消Kalman濾波引入遺忘因子來抑制濾波器的記憶長度，使其充分利用當前的觀測數(shù)據(jù)，減小以前觀測量的影響。

自適應漸消Kalman濾波的基本步驟為

(6)

(7)

(8)

(9)

(10)

當系統(tǒng)模型與輸入觀測值不匹配時，將一步預測誤差協(xié)方差擴大λk倍，使當前觀測數(shù)據(jù)在狀態(tài)估計中的權(quán)重增大，從而避免了濾波器的發(fā)散。可見，遺忘因子λk的選取對自適應濾波的效果有直接影響。

(11)

(12)

根據(jù)文獻[7]中的推導，可以得到遺忘因子的表達式為

(13)

當濾波代入的過程噪聲協(xié)方差很小時，可以近似得到遺忘因子

(14)

在差分定位中，每一顆衛(wèi)星的觀測量都可以間接反映流動站載體的位置和速度，因此只利用部分觀測量新息計算出的遺忘因子λk也能夠反映載體的動態(tài)變化。基于此，本文提出了采用獨立滑動窗的方式收集每顆衛(wèi)星的新息。收集新息并計算遺忘因子的方法為：

1)確定滑動窗總長度N。對于每顆衛(wèi)星的每種觀測量都建立長度為N的滑動窗。

2)計算當前歷元各顆衛(wèi)星的觀測量對應的新息。若相對于上一歷元參考星發(fā)生變化，則根據(jù)雙差運算的定義對所有滑動窗進行新息的轉(zhuǎn)換，轉(zhuǎn)換方法見本節(jié)下文中所述。

3)若當前時刻某觀測量的新息zk對應的滑動窗長度n

5)根據(jù)式(16)或式(17)，計算遺忘因子λk。返回步驟2)，計算下一歷元的新息和遺忘因子。

在步驟1)中，需要根據(jù)實際載體運動的先驗知識選擇合適的窗長度。如果載體在運動過程中機動較少，或所選擇的動態(tài)模型與載體實際運動狀態(tài)相近，則應選擇較長的滑動窗，得到相對平滑的濾波結(jié)果；如果載體存在較大機動，或其運動狀態(tài)未知，則應采用較短的滑動窗，增強濾波器的跟蹤性能。通過步驟3)對滑動窗的處理，保證了目前收集的新息序列依次對應相同的歷元時刻，并在步驟4)中選出滿足計算協(xié)方差條件的新息序列。

下面舉例說明步驟2)中新息的轉(zhuǎn)換方法。

假設基準站和流動站同步觀測了衛(wèi)星號分別為1、2、3、4的4顆衛(wèi)星，當參考星由1號衛(wèi)星變?yōu)?號衛(wèi)星時，新息調(diào)整情況如表1所示。

表1 參考星變化引起的新息調(diào)整

3 Kalman濾波算法在RTK中的應用

在RTK技術(shù)的實際應用中，需要固定模糊度的情況，有初始解算模糊度、發(fā)生周跳、跟蹤到新的衛(wèi)星信號等。除了初始時刻，上述其他情況都會不定時出現(xiàn)。因此，為了快速得到這些模糊度的浮點解，在動態(tài)定位中需要實時地利用Kalman濾波器進行參數(shù)估計。

基于新濾波算法的動態(tài)差分定位的流程圖如圖1所示。圖1中，初始化的濾波器參數(shù)包括狀態(tài)向量的初始值及其初始協(xié)方差陣，過程噪聲協(xié)方差，觀測噪聲協(xié)方差以及滑動窗長度；觀測量預處理包括衛(wèi)星位置計算、周跳探測等。初始模糊度固定之后，若沒有新的模糊度需要固定，則利用載波相位觀測值計算基線向量的固定解；若發(fā)生周跳、新衛(wèi)星跟蹤等情況時，則利用Kalman濾波得到的模糊度浮點解及其協(xié)方差矩陣進行搜索，根據(jù)搜索的成功與否選擇輸出固定解還是浮點解。

可見，將Kalman濾波算法實時地應用于動態(tài)差分定位中，能夠為各歷元時刻提供浮點解。初始模糊度固定之后，當由于周跳等原因出現(xiàn)需要固定的模糊度時，可以快速得到相應模糊度的浮點解及其協(xié)方差矩陣。該方法既避免了周跳修復帶來的準確性問題，又解決了多次解方程導致的計算量過大。

4 仿真驗證

4.1 仿真場景

為了驗證Kalman濾波在RTK中的應用效果，設計了高動態(tài)仿真場景，通過Matlab軟件模擬高動態(tài)載體(如某高速飛行器等)的運動，計算出北斗二代導航系統(tǒng)差分定位所需的各時刻的偽距和載波相位觀測量。

仿真場景的觀測時間從2016年4月15日13時2分0秒到當日的13時3分39.8秒，歷元間間隔為0.2s，共500個歷元。基準站坐標為東經(jīng)116.1528813°，北緯39.8121276°，高程74.48m，流動站的初始坐標為東經(jīng)116.1526479°，北緯39.8112269°，高程74.48m。在13時2分0秒時流動站向西做勻速運動，速度為1600m/s，加速度為0；前20s，流動站的加加速度為8m/s3，方向向東；第20s到第30s流動站的加加速度變?yōu)?，做加速度為160m/s2方向向東的勻加速運動；從第30s開始，流動站處于勻速運動狀態(tài)，速度為1600m/s，方向向東，直至場景結(jié)束。

觀測量噪聲均為均值為0的高斯白噪聲，其中偽距觀測量噪聲的標準差為0.5m，載波相位觀測量噪聲的標準差為0.006m。仿真中只使用北斗系統(tǒng)的B1頻點的觀測量。這些觀測量均不包含接收機鐘差、衛(wèi)星鐘差、電離層延遲、對流層延遲等誤差。對于雙差模型，這些誤差在中短基線情況下基本都可以抵消。

4.2 濾波參數(shù)的實時估計

利用本文介紹的等速模型普通Kalman濾波算法和自適應漸消Kalman濾波方法分別進行位置、速度和單差模糊度參數(shù)估計。其中，算法I表示基于等速模型的普通Kalman濾波算法，算法Ⅱ表示本文介紹的自適應漸消Kalman濾波算法。由于載體在運動過程中存在加速度和加加速度，算法Ⅱ應采用較短的滑動窗，窗長度為10個歷元。兩種算法得到的位置和速度誤差曲線如圖2和圖3所示。

圖2和圖3中，x、y、z方向分別表示CGCS-2000坐標系下的3個坐標軸的正向，與模型中的位置狀態(tài)參數(shù)表示的含義相同。可以看出，算法Ⅱ在整個觀測過程中可以實時得到比較準確的位置估計值，而算法Ⅰ的位置誤差隨著載體加速度的增大而變大。在前30s加速度不為0時，算法Ⅱ的速度濾波結(jié)果有一定的偏差，并且該偏差隨著加速度的增大而增大。30s后流動站載體做勻速運動時，可以得到比較精確的速度估值。而算法I在載體存在加速度時，無法得到準確的速度結(jié)果。

兩種算法得到的模糊度誤差曲線分別如圖4和圖5所示。

圖4、圖5中的4條曲線分別表示衛(wèi)星號為3、5、7、12的衛(wèi)星與8號衛(wèi)星組成的雙差模糊度誤差。根據(jù)仿真場景設定，基準站在第16s和第40s才分別跟蹤到5號衛(wèi)星和12號衛(wèi)星。在這2個時刻之前，它們對應的單差模糊度參數(shù)為0，在觀測到它們之前圖中未畫出其誤差曲線。當獲得它們的觀測量后，才能夠得到其雙差模糊度誤差曲線。3號衛(wèi)星和12號衛(wèi)星的載波相位觀測值分別在第14s和第60s出現(xiàn)了野值；第24s，7號衛(wèi)星出現(xiàn)了大小為10周的周跳；第50s，3號衛(wèi)星出現(xiàn)了大小為8周的周跳。當某顆衛(wèi)星發(fā)生周跳后，它的模糊度誤差曲線應收斂到其周跳的周數(shù)。因此，第24s后7號衛(wèi)星的模糊度誤差曲線最終收斂到10，第50s后3號衛(wèi)星的模糊度誤差曲線最終收斂到8。可見，當發(fā)生周跳、出現(xiàn)野值或跟蹤到新衛(wèi)星信號時，算法I在載體存在加加速度時，模糊度曲線都出現(xiàn)了較大跳變，收斂速度較慢；算法Ⅱ的模糊度參數(shù)可以快速收斂到正確值，并且收斂速度不受加速度和加加速度的影響。因此，當出現(xiàn)上述現(xiàn)象時，通過算法Ⅱ可以直接利用濾波估計得到的雙差模糊度浮點解進行搜索，避免了復雜的處理過程，保證了基線解算的連續(xù)可靠。

4.3 自適應漸消算法對模糊度固定的改善

再次建立4.1節(jié)中的仿真場景，且整個觀測時段不存在周跳、野值以及新衛(wèi)星跟蹤等情況。比較兩種算法得到的模糊度浮點解及其協(xié)方差矩陣對模糊度固定的影響。

對濾波得到的每個歷元的浮點解及其協(xié)方差陣采用LAMBDA算法進行降相關(guān)變換和整數(shù)搜索，分別統(tǒng)計兩種算法對應的整數(shù)搜索執(zhí)行時間和固定成功率，其結(jié)果如表2所示。

表2 兩種算法對應的模糊度固定結(jié)果

表2中的成功率是用模糊度搜索成功的歷元數(shù)除以總歷元數(shù)得到的，其中總歷元數(shù)包括濾波器未收斂時的歷元，因此該成功率并不是接近100%，而濾波器收斂后即可成功固定。表2中的平均搜索時間是指Matlab軟件在每個歷元執(zhí)行整數(shù)搜索程序所用時間的算術(shù)平均值。模糊度搜索采用的是橢球空間逐步縮小的方法，初始的搜索空間大小為無窮大，無需人為設定。因此，整數(shù)搜索時間與搜索參數(shù)設置無關(guān)。

兩種算法的每個歷元的搜索時間如圖6所示。

可見，算法Ⅱ在固定成功率和搜索時間上都明顯優(yōu)于算法I。從圖6中可以看出，當流動站加速度較大時，算法Ⅱ在搜索時間上具有明顯優(yōu)勢。而當流動站變?yōu)閯蛩龠\動時，搜索時間才逐漸趨于一致。

上述結(jié)果證明，自適應漸消算法相對于一般Kalman濾波算法，在高動態(tài)下提高了模糊度浮點解的精度，在提高了搜索成功率的同時，縮小了整數(shù)搜索空間，使得整數(shù)搜索時間減少，提高了解算效率。

5 結(jié)論

本文提出了一種應用于RTK技術(shù)中的Kalman濾波方法。它不僅能夠為初始模糊度解算提供浮點解，而且在發(fā)生周跳、衛(wèi)星失鎖時實時提供相應衛(wèi)星的模糊度浮點解，起到周跳修復的作用。此外，通過引入遺忘因子實時調(diào)整當前觀測量在濾波過程中所占的比重，使算法能夠適應更復雜的動態(tài)環(huán)境；利用獨立滑動窗收集新息，解決了由于觀測量中斷或參考星變化無法計算新息協(xié)方差的問題。實驗分析表明，該濾波方法能夠得到精度較高的位置和速度浮點解，并且在發(fā)生周跳、野值和跟蹤到新衛(wèi)星時，相應模糊度參數(shù)能夠快速收斂到真值；此外，由于采用了自適應漸消算法，高動態(tài)下提高了模糊度浮點解精度，縮小了整數(shù)搜索空間，提高了搜索效率和成功率。