萇永娜， 張 海， 李玉潔， 楊 靜

(北京航空航天大學(xué)控制一體化技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京 100191)

0 引言

在觀測(cè)噪聲動(dòng)態(tài)估計(jì)研究中，文獻(xiàn)［1］提出了自適應(yīng)更新?tīng)顟B(tài)噪聲、觀測(cè)噪聲誤差統(tǒng)計(jì)特性的方法;文獻(xiàn)［2］提出了基于新息序列的自適應(yīng)開(kāi)窗逼近法，依據(jù)觀測(cè)信息自適應(yīng)調(diào)整狀態(tài)協(xié)方差陣和觀測(cè)協(xié)方差陣;文獻(xiàn)［3］提出了一種動(dòng)態(tài)自適應(yīng)抗差濾波算法，能夠自適應(yīng)地分配觀測(cè)信息和模型信息對(duì)狀態(tài)估計(jì)的權(quán)重。

對(duì)于線性系統(tǒng)而言，上述觀測(cè)噪聲特性統(tǒng)計(jì)方法均依賴(lài)于新息量，如果一步狀態(tài)預(yù)測(cè)值估計(jì)不準(zhǔn)確，觀測(cè)噪聲特性的估計(jì)會(huì)出現(xiàn)偏差，從而影響濾波精度。

本文從組合導(dǎo)航系統(tǒng)中不同測(cè)量系統(tǒng)的測(cè)量互補(bǔ)特性出發(fā)，在一定情況下，通過(guò)相對(duì)測(cè)量關(guān)系的構(gòu)建，實(shí)現(xiàn)觀測(cè)噪聲的動(dòng)態(tài)估計(jì)，在此基礎(chǔ)上，以預(yù)設(shè)濾波精度為指標(biāo)，通過(guò)構(gòu)造自適應(yīng)因子對(duì)估計(jì)觀測(cè)噪聲進(jìn)行適當(dāng)?shù)恼{(diào)節(jié)，最終實(shí)現(xiàn)高精度的自適應(yīng)濾波。

1 基于R陣動(dòng)態(tài)估計(jì)的自適應(yīng)濾波算法理論分析

1.1 現(xiàn)有自適應(yīng)濾波算法分析

標(biāo)準(zhǔn)卡爾曼濾波(SKF)中，狀態(tài)估計(jì)和濾波增益為

自適應(yīng)濾波是通過(guò)估計(jì)或修正系統(tǒng)模型參數(shù)、噪聲特性或?yàn)V波增益等方法來(lái)提高濾波精度的，例如漸消濾波［4］和抗差自適應(yīng)濾波［5-7］在SKF基礎(chǔ)上的改進(jìn)如下所述。

漸消濾波增益陣為

其中:Φk，k-1為系統(tǒng)一步轉(zhuǎn)移矩陣;Qk為系統(tǒng)噪聲方差陣;Pk-1為狀態(tài)估計(jì)方差陣。

抗差自適應(yīng)濾波增益陣為

漸消因子λ與抗差自適應(yīng)因子γ分別為

其中:tr［·］為矩陣求跡符號(hào);c0為常值。

漸消濾波將驗(yàn)前狀態(tài)協(xié)方差矩陣膨脹λ(λ≥1)倍，抗差自適應(yīng)濾波通過(guò)構(gòu)造自適應(yīng)因子γ(0＜γ≤1)來(lái)調(diào)節(jié)狀態(tài)噪聲的協(xié)方差陣，由式(3)、式(4)看出，兩者的最終結(jié)果均是將濾波增益陣K增大，從而提高觀測(cè)信息的利用權(quán)重，但當(dāng)觀測(cè)誤差變大時(shí)，其濾波效果將明顯變差。由式(5)、式(6)知，λ與γ的構(gòu)造均是新息量的函數(shù)，一旦狀態(tài)估計(jì)值有較大誤差，對(duì)觀測(cè)噪聲的估計(jì)會(huì)出現(xiàn)偏差，從而影響自適應(yīng)濾波的估計(jì)精度和穩(wěn)定性。

1.2 觀測(cè)噪聲R自適應(yīng)估計(jì)方法分析

文獻(xiàn)［8］針對(duì)GPS/DR系統(tǒng)提出了一種改進(jìn)的航向估計(jì)算法，利用GPS和陀螺儀信息自適應(yīng)估計(jì)航向測(cè)量噪聲方差陣R。文獻(xiàn)［9］提出了一種基于不同測(cè)量系統(tǒng)測(cè)量特性的觀測(cè)噪聲方差陣R自適應(yīng)估計(jì)定理，并從理論上證明了其正確性。設(shè)對(duì)于同一物理量Z(k)同時(shí)存在兩種不同的測(cè)量系統(tǒng)M1、M2，并且兩系統(tǒng)的測(cè)量特性互補(bǔ)。其中，測(cè)量系統(tǒng)M1的測(cè)量結(jié)果在真值附近抖動(dòng)，在某些情況下其抖動(dòng)幅度較大，而M2具有短時(shí)高精度，且其測(cè)量值的變化趨勢(shì)與真實(shí)值的變化趨勢(shì)基本一致。設(shè)兩系統(tǒng)測(cè)量值分別為Z1(k)、Z2(k)，單測(cè)量系統(tǒng)自差分序列分別為ΔZ1(k)、Z2(k)，且

則M1的測(cè)量噪聲方差陣R可進(jìn)行如下近似估計(jì):

對(duì)于測(cè)量噪聲方差陣R的估計(jì)，文獻(xiàn)［9］中提出了一種連續(xù)滑動(dòng)窗口的統(tǒng)計(jì)方法，在GPS/INS機(jī)載導(dǎo)航系統(tǒng)中進(jìn)行了仿真驗(yàn)證，當(dāng)測(cè)量噪聲方差陣R滿足在一定區(qū)間內(nèi)近似為同分布的條件時(shí)，此方法能夠獲得良好的估計(jì)效果。但是，滑動(dòng)窗口的寬度在實(shí)際應(yīng)用中難以確定，并且若窗口內(nèi)只有當(dāng)前歷元的觀測(cè)誤差很大，由于使用了大量歷史數(shù)據(jù)的平滑作用，會(huì)使當(dāng)前歷元觀測(cè)噪聲的估計(jì)值降低，從而無(wú)法獲得當(dāng)前歷元狀態(tài)量的最優(yōu)估計(jì)。此外，此方法也在一定程度上增加了計(jì)算量和存貯量。

1.3 基于R陣動(dòng)態(tài)估計(jì)的自適應(yīng)濾波算法

在實(shí)際系統(tǒng)中，觀測(cè)噪聲的動(dòng)態(tài)特性通常是復(fù)雜多變的，為如實(shí)反映各個(gè)歷元瞬時(shí)噪聲特性，本算法基于R自適應(yīng)估計(jì)定理的思想，針對(duì)單次歷元的觀測(cè)噪聲特性進(jìn)行動(dòng)態(tài)估計(jì)。事實(shí)上，基于不同測(cè)量特性的R估值定理可以獲得接近實(shí)際系統(tǒng)的測(cè)量噪聲特性(文獻(xiàn)［9］已進(jìn)行了仿真驗(yàn)證)，但若直接作用于濾波解算中，濾波效果并不十分理想。

在卡爾曼濾波中，式(3)第二項(xiàng)體現(xiàn)了修正信息對(duì)狀態(tài)估計(jì)的貢獻(xiàn)，其權(quán)重由Kk確定。對(duì)于觀測(cè)信息Zk，基于雙測(cè)量系統(tǒng)的R估計(jì)定理主要針對(duì)觀測(cè)噪聲的隨機(jī)誤差進(jìn)行估計(jì)，并未考慮系統(tǒng)誤差;對(duì)于Hk，由于設(shè)計(jì)卡爾曼濾波器時(shí)選取的系統(tǒng)模型往往包含一定的不確定性，因此也包含了一定的誤差，從而造成修正信息可信度的下降。Kk是建立在P和R均可精確獲得的理想情況下的最優(yōu)增益，而這在實(shí)際系統(tǒng)中難以實(shí)現(xiàn)。因此為減小狀態(tài)估計(jì)誤差，本文從預(yù)設(shè)的定位精度ε出發(fā)，在當(dāng)前濾波增益陣Kk的基礎(chǔ)上乘一個(gè)自適應(yīng)因子α(0＜α≤1)，等價(jià)于將估計(jì)R進(jìn)行一定程度的膨脹作用(設(shè)為β倍，β≥1)，將系統(tǒng)誤差、模型誤差的影響包含進(jìn)來(lái)，從而得到精度更高的濾波估計(jì)。

由于狀態(tài)向量通常含有不同類(lèi)型的參數(shù)，且各觀測(cè)分量對(duì)同一狀態(tài)量的作用也不盡相同，因此本文采用序貫處理方法對(duì)各觀測(cè)分量進(jìn)行處理。設(shè)Rk為對(duì)角陣(在實(shí)際系統(tǒng)中具有一定的普遍性)，主對(duì)角線元素，…，k均為一維變量。依據(jù)序貫處理思想，設(shè)k時(shí)刻，對(duì)于第i個(gè)觀測(cè)分量，有

定義自適應(yīng)因子為α=diag［α1α2… αm］，相應(yīng)地 β=diag［β1β2… βm］。

因此新的濾波增益陣也可以表示為

由式(12)及式(14)聯(lián)立可得

因αi不為零，式(15)兩邊分別求逆，得

由于0＜αi≤1，最終新的噪聲陣為

2 基于R陣動(dòng)態(tài)估計(jì)的自適應(yīng)濾波算法實(shí)現(xiàn)

1)單測(cè)量系統(tǒng)自差分。

設(shè)k時(shí)刻，如第1.2節(jié)所述，組合導(dǎo)航系統(tǒng)中兩測(cè)量系統(tǒng)的測(cè)量值分別為Z1(k)、Z2(k)，首先對(duì)兩測(cè)量系統(tǒng)分別求單系統(tǒng)自差分序列ΔZ1(k)、ΔZ2(k)為

2)雙測(cè)量系統(tǒng)互差分。

3)雙測(cè)量系統(tǒng)互差分序列單歷元估計(jì)。

4)自適應(yīng)因子的構(gòu)造。

設(shè)組合系統(tǒng)需要達(dá)到的定位精度為ε，測(cè)量噪聲估計(jì)分量值為R^ik，基于測(cè)量噪聲估計(jì)方差陣映射的觀測(cè)誤差水平，自適應(yīng)因子αi可構(gòu)造為

其中，c為常數(shù)，通常取為0.2 ～0.8。

5)本自適應(yīng)濾波的序貫處理核心算法。

同SKF相比，本自適應(yīng)算法僅增加了各歷元雙系統(tǒng)互差分及自適應(yīng)因子的簡(jiǎn)單計(jì)算，并且采用序貫處理方法，對(duì)Zk各分量進(jìn)行順序處理，將對(duì)高階矩陣的求逆轉(zhuǎn)變?yōu)榈碗A矩陣的求逆，有效降低了計(jì)算量。

3 算法驗(yàn)證

3.1 GPS/DR組合導(dǎo)航濾波模型

為驗(yàn)證本算法的有效性，本算例在GPS/DR實(shí)際系統(tǒng)中，在多遮擋、多障礙物的路段，對(duì)機(jī)車(chē)進(jìn)行高精度定位。本文采取直接法建模，選取的狀態(tài)向量為，各分量依次為北、東向位置，航向角，速度，航向角速度，陀螺儀零偏B，里程儀刻度因子S;選取的觀測(cè)向量為Z=，各分量依次為 GPS測(cè)量的北、東向位置、航向，里程儀測(cè)量的速度，陀螺儀測(cè)量的航向角速度。

本文采用EKF方法進(jìn)行線性化，得出的狀態(tài)轉(zhuǎn)移矩陣Φk+1，k及觀測(cè)矩陣Hk如下:

其中:T為濾波周期;Pod為里程儀測(cè)量的脈沖數(shù)。

3.2 算例分析

3.2.1 實(shí)驗(yàn)條件

機(jī)車(chē)運(yùn)行初始條件:位置為北緯36.598090°、東經(jīng)114.451120°，航向角為 322.0755°，速度為 2.7344 m/s。GPS采用差分定位，為實(shí)現(xiàn)高精度定位，本實(shí)驗(yàn)中將經(jīng)緯度坐標(biāo)轉(zhuǎn)換為WGS-84直角坐標(biāo)系。本實(shí)驗(yàn)中，機(jī)車(chē)在鋼廠的鐵軌上運(yùn)行，相鄰鐵軌大約為5 m，并且由于工況要求，機(jī)車(chē)運(yùn)行速度較低，主要對(duì)東、北向位置進(jìn)行估計(jì)，定位精度要求為5 m。

3.2.2 對(duì)比算法

除標(biāo)準(zhǔn)卡爾曼濾波以外，本算法與改進(jìn)的sage-husa自適應(yīng)算法［9］及 MAKF［10］算法進(jìn)行了同條件對(duì)比實(shí)驗(yàn)。改進(jìn)的sage-husa自適應(yīng)算法只針對(duì)觀測(cè)噪聲R的統(tǒng)計(jì)特性進(jìn)行自適應(yīng)估計(jì)。MAKF算法中，GPS測(cè)量噪聲的估計(jì)方法為

其中:i為當(dāng)前濾波時(shí)刻;Ck為雙系統(tǒng)互差分序列;M為滑動(dòng)窗口寬度，此處取為30。

3.2.3 仿真結(jié)果分析

在實(shí)驗(yàn)條件下，GPS測(cè)量結(jié)果如圖1所示。150～174 s、230～245 s區(qū)段內(nèi)GPS信號(hào)良好，定位結(jié)果正常;175～229 s區(qū)段內(nèi)GPS由于受到周?chē)邔咏ㄖ锏挠绊?，輸出結(jié)果明顯偏離真實(shí)軌道。采用SKF、改進(jìn)的sage-husa算法、MAKF算法及本自適應(yīng)算法進(jìn)行GPS/DR組合導(dǎo)航濾波解算，實(shí)驗(yàn)結(jié)果如圖2及圖3所示。其中本算法構(gòu)造自適應(yīng)因子時(shí)，經(jīng)過(guò)反復(fù)試驗(yàn)，式(21)中常數(shù)c最終取為0.2。與其他各算法相比，本算法能夠自適應(yīng)地跟蹤周?chē)h(huán)境的變化，輸出結(jié)果光滑連續(xù)，最大定位誤差為4 m，滿足預(yù)設(shè)精度要求。

圖1 GPS測(cè)量結(jié)果Fig.1 The output of GPS

從圖2a、圖2c中可以看出，SKF中觀測(cè)噪聲的統(tǒng)計(jì)特性由驗(yàn)前統(tǒng)計(jì)結(jié)果獲得，其結(jié)果恒定不變(10 m2)，其他算法可對(duì)觀測(cè)噪聲進(jìn)行自適應(yīng)估計(jì)。175～198 s時(shí)間段內(nèi)，GPS的噪聲特性實(shí)際上已經(jīng)發(fā)生了較大變化，但是改進(jìn)的sage-husa算法、MAKF算法均未做出響應(yīng);200～230 s時(shí)間段內(nèi)，在GPS的定位誤差較大的情況下，雖然改進(jìn)的sage-husa算法、MAKF算法對(duì)GPS的噪聲特性進(jìn)行了自適應(yīng)估計(jì)，但估計(jì)動(dòng)態(tài)性能較差;231～245 s時(shí)間段內(nèi)，GPS已恢復(fù)正常定位，而改進(jìn)的sage-husa算法、MAKF算法對(duì)GPS觀測(cè)噪聲的估計(jì)仍嚴(yán)重偏高;整個(gè)時(shí)間段內(nèi)，本自適應(yīng)濾波算法快速、及時(shí)地跟蹤了GPS噪聲特性的變化。

由于改進(jìn)的sage-husa算法在對(duì)R更新時(shí)耦合了狀態(tài)估計(jì)誤差的影響，因而估計(jì)結(jié)果不可靠。MAKF算法對(duì)R的估計(jì)建立在窗口內(nèi)各歷元的統(tǒng)計(jì)結(jié)果的基礎(chǔ)上，因而會(huì)受到歷史數(shù)據(jù)平滑作用的影響，削弱了對(duì)觀測(cè)噪聲估計(jì)的動(dòng)態(tài)性能，從而使其在高動(dòng)態(tài)的實(shí)際系統(tǒng)中的應(yīng)用受到限制。本自適應(yīng)算法針對(duì)單次歷元的觀測(cè)噪聲特性進(jìn)行估計(jì)，避免受到大量過(guò)去數(shù)據(jù)的影響，且在對(duì)R的估計(jì)過(guò)程中不存在與狀態(tài)估計(jì)誤差協(xié)方差陣P的耦合關(guān)系;從預(yù)設(shè)濾波精度出發(fā)，基于估計(jì)的觀測(cè)噪聲水平確定自適應(yīng)調(diào)節(jié)因子的比例，對(duì)當(dāng)前估計(jì)觀測(cè)噪聲信息進(jìn)行適當(dāng)?shù)呐蛎涀饔?，相?dāng)于將觀測(cè)估計(jì)誤差、狀態(tài)估計(jì)誤差等包含進(jìn)來(lái)，從而實(shí)現(xiàn)了準(zhǔn)確、可靠的濾波估計(jì)效果。

圖2 4種算法的R估計(jì)結(jié)果Fig.2 Comparison of four Kalman filtering algorithms in estimating R

圖3 4種算法實(shí)驗(yàn)結(jié)果對(duì)比Fig.3 Comparison of four Kalman filtering algorithms

4 結(jié)論

本算法基于組合導(dǎo)航系統(tǒng)中不同測(cè)量系統(tǒng)的測(cè)量特性，構(gòu)造相對(duì)測(cè)量關(guān)系估計(jì)單次歷元的噪聲特性，避免了現(xiàn)有估計(jì)方法中狀態(tài)誤差和歷史數(shù)據(jù)的影響;在滿足預(yù)設(shè)精度的前提下，通過(guò)構(gòu)造自適應(yīng)因子對(duì)估計(jì)R陣適當(dāng)調(diào)節(jié)，收斂于濾波增益K陣對(duì)觀測(cè)信息有效利用的準(zhǔn)則，有效地提高了濾波精度。同改進(jìn)的sage-husa算法、MAKF等現(xiàn)有自適應(yīng)濾波方法相比，本自適應(yīng)算法在實(shí)際系統(tǒng)中得到了良好的應(yīng)用，在濾波精度和抗干擾性能上，具有明顯的優(yōu)越性，具有深入的理論推廣意義和工程實(shí)用價(jià)值。此外，由于組合導(dǎo)航系統(tǒng)中通常存在某些兩個(gè)測(cè)量系統(tǒng)及以上的物理量，因此本文主要針對(duì)雙測(cè)量系統(tǒng)的測(cè)量特性進(jìn)行自適應(yīng)估計(jì)，對(duì)于只有單個(gè)測(cè)量系統(tǒng)的物理量，可以考慮構(gòu)造等效觀測(cè)量的方法進(jìn)行估計(jì)，值得進(jìn)一步研究。

［1］SAGE A P,HUSA G W.Adaptive filtering with unknown prior statistic［C］//Proceedings of Joint Automatic Control Conference，Washington DC:American Automatic Control Council,1969:760-769.

［2］MEHRA R K.On the identification of variances and Adaptive Kalman filtering［J］.IEEE Transactionson Automatic Control,1970,15(2):175-184.

［3］CUI X，YANG Y.Adaptively robust filtering with classified adaptive factors［J］.Progress in Natural Science,2006,16(8):846-851.

［4］王虎，王解先，白貴霞，等.改進(jìn)的漸消卡爾曼在GPS動(dòng)態(tài)定位中的應(yīng)用［J］.同濟(jì)大學(xué)學(xué)報(bào)，2011，39(1):124-128.

［5］YANG Yuanxi,GAO Weiguang.Comparison of two fading filters and adaptively robust filter［J］.Geo-spatial Information Science,2007,10(3):200-203.

［6］YANG Y,GAO W.An optimal adaptive Kalman filter［J］.Journal of Geodesy,2006,80(4):177-183.

［7］YANG Yuanxi,GAO Weiguang.A new learning statistic for adaptive filter based on predicted residual［J］.Progress in Natural Science,2006,16(8):833-837.

［8］劉倩，張海，周艷麗，等.改進(jìn)的GPS/DR組合導(dǎo)航自適應(yīng)航向估計(jì)算法［J］.電光與控制，2010，17(6):71-77.

［9］張海，常艷紅，車(chē)歡.基于GPS/INS不同測(cè)量特性的自適應(yīng)卡爾曼濾波算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2010，18(6):331-336.

［10］常艷紅.GPS/INS組合導(dǎo)航自適應(yīng)濾波技術(shù)研究［D］.北京:北京航空航天大學(xué)，2010.

［11］WANG Jian,LIU Jiang,CAI Bogen.Study on information fusion algorithm in embedded integrated navigation system［C］//2008 International Conference on Intelligent Computation Technology and Automation,2008:1007-1010.