自適應(yīng)漸消Kalman濾波算法在RTK中的應(yīng)用研究

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

(北京自動(dòng)化控制設(shè)備研究所，北京 100074)

0 引言

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

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

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

1 RTK中的Kalman濾波模型

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

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

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

(1)

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

(2)

其中，I為3×3的單位陣，ts為2個(gè)觀測(cè)歷元之間的時(shí)間間隔。

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

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

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

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

(3)

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

(4)

其對(duì)應(yīng)的觀測(cè)噪聲協(xié)方差矩陣R為

(5)

2 自適應(yīng)漸消Kalman濾波算法

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

自適應(yīng)漸消Kalman濾波的基本步驟為

(6)

(7)

(8)

(9)

(10)

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

(11)

(12)

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

(13)

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

(14)

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

1)確定滑動(dòng)窗總長(zhǎng)度N。對(duì)于每顆衛(wèi)星的每種觀測(cè)量都建立長(zhǎng)度為N的滑動(dòng)窗。

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

3)若當(dāng)前時(shí)刻某觀測(cè)量的新息zk對(duì)應(yīng)的滑動(dòng)窗長(zhǎng)度n

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

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

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

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

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

3 Kalman濾波算法在RTK中的應(yīng)用

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

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

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

4 仿真驗(yàn)證

4.1 仿真場(chǎng)景

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

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

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

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

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

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

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

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

4.3 自適應(yīng)漸消算法對(duì)模糊度固定的改善

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

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

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

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

兩種算法的每個(gè)歷元的搜索時(shí)間如圖6所示。

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

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

5 結(jié)論

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