基于自適應(yīng)EKF的相對(duì)導(dǎo)航算法研究

雷創(chuàng)

（中國(guó)電子科技集團(tuán)公司第二十研究所，西安 710068）

0 引言

編隊(duì)飛機(jī)的相對(duì)導(dǎo)航功能將在未來戰(zhàn)爭(zhēng)中發(fā)揮至關(guān)重要的作用，可有效彌補(bǔ)絕對(duì)導(dǎo)航定位性能較差的缺陷。基于編隊(duì)飛機(jī)間的測(cè)距測(cè)角功能設(shè)備，可在不依賴外界條件下實(shí)現(xiàn)平臺(tái)間自主的相對(duì)導(dǎo)航功能。測(cè)距測(cè)角是一種非線性的測(cè)量信息，常規(guī)卡爾曼技術(shù)無法對(duì)其濾波處理。擴(kuò)展卡爾曼濾波是基于一階泰勒級(jí)數(shù)展開的近似線性化濾波技術(shù)，是工程中最常用的非線性系統(tǒng)濾波技術(shù)之一。擴(kuò)展卡爾曼濾波中需要知道精確的系統(tǒng)模型和噪聲統(tǒng)計(jì)先驗(yàn)知識(shí)，實(shí)際濾波中這些條件較難滿足，往往使得濾波結(jié)果往往不是最優(yōu)的，且不經(jīng)過特殊的處理，極易導(dǎo)致濾波發(fā)散。實(shí)際應(yīng)用中多采用自適應(yīng)卡爾曼濾波方法，例如：強(qiáng)跟蹤卡爾曼濾波、基于極大似然準(zhǔn)則的自適應(yīng)卡爾曼濾波、Sage-Husa自適應(yīng)卡爾曼濾波等。

通常情況下，基于Sage-Husa的自適應(yīng)時(shí)變?cè)肼暪烙?jì)濾波方法具有與常規(guī)卡爾曼濾波方法相當(dāng)?shù)木惹揖哂杏?jì)算量小、效率高等優(yōu)點(diǎn)，得到最為廣泛的研究，但其不具有對(duì)測(cè)量噪聲方差陣和系統(tǒng)噪聲方差陣進(jìn)行同時(shí)在線估計(jì)的能力。因此，本文研究基于自適應(yīng)擴(kuò)展卡爾曼濾波的相對(duì)導(dǎo)航濾波算法，即在對(duì)測(cè)量數(shù)據(jù)進(jìn)行遞推濾波的同時(shí)，利用Sage-Husa法對(duì)測(cè)量噪聲方差陣進(jìn)行動(dòng)態(tài)估計(jì)，實(shí)時(shí)修正測(cè)量噪聲的統(tǒng)計(jì)特性，而系統(tǒng)噪聲則是由編隊(duì)飛機(jī)慣導(dǎo)特性的先驗(yàn)值獲得，由此通過調(diào)節(jié)濾波增益參數(shù)達(dá)到提高相對(duì)定位精度且抑制濾波發(fā)散的目的。

1 系統(tǒng)模型

以一架長(zhǎng)機(jī)和一架僚機(jī)組成的雙機(jī)編隊(duì)為例（參見圖1），在不依賴絕對(duì)導(dǎo)航信息的情況下，長(zhǎng)機(jī)通過相對(duì)導(dǎo)航功能實(shí)時(shí)獲知僚機(jī)高精度的相對(duì)位置信息。此中，相對(duì)導(dǎo)航坐標(biāo)系定義為：以長(zhǎng)機(jī)質(zhì)心o為原點(diǎn)，x沿長(zhǎng)機(jī)水平面指向東向，y沿長(zhǎng)機(jī)水平面指向真北，z沿長(zhǎng)機(jī)當(dāng)?shù)卮咕€指向天向，相對(duì)導(dǎo)航坐標(biāo)系隨著長(zhǎng)機(jī)的運(yùn)動(dòng)而移動(dòng)。

測(cè)距測(cè)角功能設(shè)備的測(cè)量信息描述為：測(cè)量的距離ρ為兩架飛機(jī)的徑向連線；測(cè)量的相對(duì)方位角α為兩架飛機(jī)的徑向連線在長(zhǎng)機(jī)本地水平面的投影與長(zhǎng)機(jī)真北向的夾角；測(cè)量的相對(duì)俯仰角β為兩架飛機(jī)的徑向連線與長(zhǎng)機(jī)本地水平面的夾角。相對(duì)導(dǎo)航的測(cè)量信息是極坐標(biāo)系下的位置信息，濾波中需要對(duì)該信息進(jìn)行線性化處理。

圖1 相對(duì)導(dǎo)航坐標(biāo)系與測(cè)量信息的關(guān)系示意圖

本文研究中采用簡(jiǎn)化的相對(duì)導(dǎo)航模型，離散化的系統(tǒng)狀態(tài)方程為：

系統(tǒng)狀態(tài)矢量為

表示僚機(jī)在相對(duì)導(dǎo)航坐標(biāo)系下的三維位置、速度及加速度。

相對(duì)導(dǎo)航的系統(tǒng)狀態(tài)矢量通過將雙機(jī)的慣導(dǎo)信息相差獲得，即：將僚機(jī)和長(zhǎng)機(jī)慣導(dǎo)提供的經(jīng)緯高位置和、地理系速度和以及地理系加速度和分別轉(zhuǎn)換為地心系下的狀態(tài)矢量，然后相差并與轉(zhuǎn)換矩陣S相乘，由此獲得相對(duì)導(dǎo)航坐標(biāo)系下的系統(tǒng)狀態(tài)矢量。其中，轉(zhuǎn)換矩陣S為：

式中，1L、1λ分別為長(zhǎng)機(jī)的經(jīng)度、緯度。狀態(tài)矢量求取過程中，僚機(jī)的慣導(dǎo)信息是通過數(shù)據(jù)鏈發(fā)送至長(zhǎng)機(jī)，研究中假設(shè)時(shí)延問題已通過時(shí)間對(duì)準(zhǔn)手段進(jìn)行了同步，測(cè)量信息和狀態(tài)信息都為同一時(shí)間的傳感器測(cè)量信息。

狀態(tài)轉(zhuǎn)移矩陣φ表示為：

Γ為系統(tǒng)噪聲驅(qū)動(dòng)陣：

系統(tǒng)測(cè)量方程為：

其中，Zk為測(cè)距測(cè)角設(shè)備k時(shí)刻的測(cè)量信息，表示為：Z=[ρα β]T，Vk為測(cè)量噪聲且V～

階泰勒級(jí)數(shù)展開方法對(duì)測(cè)量矩陣進(jìn)行線性化，可得：

經(jīng)過線性化處理的測(cè)量方程表示為：

2 自適應(yīng)EKF算法

由于通過測(cè)距測(cè)角設(shè)備受多種因素的影響，其誤差通常具有較多的不確定性，且利用EKF線性化處理帶來的系統(tǒng)誤差以及算法本身不具有在線調(diào)整測(cè)量噪聲方差陣的能力，通常不能獲得最優(yōu)的估計(jì)結(jié)果。下面研究利用Sage-Husa自適應(yīng)方法對(duì)濾波算法進(jìn)行優(yōu)化，以期獲得最優(yōu)的系統(tǒng)狀態(tài)估計(jì)參數(shù)。

常規(guī)擴(kuò)展卡爾曼濾波處理過程如下。狀態(tài)一步預(yù)測(cè)：

均方誤差矩陣一步預(yù)測(cè)：

濾波增益：

狀態(tài)估計(jì)：

均方誤差矩陣估計(jì)：

處理過程中，采用Sage-Husa自適應(yīng)時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)方法對(duì)測(cè)量噪聲方差陣kR進(jìn)行動(dòng)態(tài)估計(jì)，即將kR調(diào)整為：

式中，d=(1-b)/(1-bk)，b為遺忘因子，通常在k-1[0 .95,0.995]間取值。當(dāng)b較小時(shí)，Rk對(duì)當(dāng)前時(shí)刻測(cè)量信息依賴的權(quán)重較大，這樣雖然使得Rk更快地調(diào)整到與實(shí)際相匹配的測(cè)量噪聲方差陣，但也使得Rk陣頻繁震蕩致使濾波結(jié)果具有一定的波動(dòng)性；當(dāng)b較大時(shí)，Rk則較為依賴歷史的測(cè)量信息，但Rk收斂時(shí)間較長(zhǎng)且容易導(dǎo)致濾波發(fā)散，因此需要合理地配置b值。

3 仿真驗(yàn)證

下面利用數(shù)字仿真對(duì)基于自適應(yīng) EKF的相對(duì)導(dǎo)航算法的效果進(jìn)行驗(yàn)證。長(zhǎng)機(jī)和僚機(jī)編隊(duì)的飛行軌跡如圖 2所示，其中長(zhǎng)機(jī)高度 8km、僚機(jī)高度6km，仿真時(shí)長(zhǎng)約 40min。仿真中，慣導(dǎo)加速度計(jì)隨機(jī)漂移50gμ，且雙機(jī)慣導(dǎo)加速度計(jì)誤差互不相關(guān)；測(cè)距誤差 3m，相對(duì)方位角及相對(duì)俯仰角誤差都為 0.2°，測(cè)量誤差均為高斯白噪聲，本文研究中不考慮測(cè)量設(shè)備系統(tǒng)誤差的影響；在700s～1000s時(shí)間段內(nèi)測(cè)量噪聲擴(kuò)大為默認(rèn)值的3倍，在1700s～2000s時(shí)間段內(nèi)測(cè)量噪聲擴(kuò)大為默認(rèn)值的5倍；測(cè)量噪聲方差陣的遺忘因子取0.98。

在上述條件下，將本文研究的自適應(yīng)EKF算法（AEKF）和常規(guī)EKF算法（EKF）進(jìn)行了比較，相對(duì)導(dǎo)航定位的仿真結(jié)果如圖3～圖5所示。

圖2 編隊(duì)飛機(jī)飛行軌跡

圖3 相對(duì)導(dǎo)航東向位置誤差

圖4 相對(duì)導(dǎo)航北向位置誤差

圖5 相對(duì)導(dǎo)航天向位置誤差

通過圖3～圖5可以看出，在測(cè)量噪聲未擴(kuò)大的情況下，本文研究的自適應(yīng)EKF算法與常規(guī)EKF算法相比，精度相當(dāng)，且相對(duì)定位結(jié)果連續(xù)平滑；當(dāng)測(cè)量誤差擴(kuò)大至3倍時(shí)，常規(guī)EKF算法的相對(duì)定位精度為15.4m（σ1），自適應(yīng)EKF相對(duì)定位精度6.5m（σ1）；當(dāng)測(cè)量誤差擴(kuò)大至5倍時(shí)，常規(guī)EKF算法的相對(duì)定位精度為24.6m（σ1），自適應(yīng)EKF相對(duì)定位精度7.2m（σ1）。由此可見，當(dāng)測(cè)量誤差變大時(shí)，基于自適應(yīng)EKF濾波算法表現(xiàn)出了良好的性能，相對(duì)定位精度明顯優(yōu)于常規(guī)濾波算法。

4 結(jié)束語

本文研究了基于自適應(yīng)擴(kuò)展卡爾曼濾波的相對(duì)導(dǎo)航算法，即利用泰勒一階展開法對(duì)測(cè)量矩陣進(jìn)行了線性化處理，并利用Sage-Husa自適應(yīng)時(shí)變?cè)肼暪烙?jì)方法對(duì)測(cè)量噪聲方差陣進(jìn)行動(dòng)態(tài)估計(jì)。通過仿真分析，本文研究的自適應(yīng)相對(duì)導(dǎo)航算法可獲得編隊(duì)飛機(jī)間高精度的相對(duì)定位信息，尤其是當(dāng)測(cè)量噪聲發(fā)生變化時(shí)，可通過動(dòng)態(tài)調(diào)整測(cè)量噪聲方差陣數(shù)值，對(duì)相對(duì)定位狀態(tài)參數(shù)進(jìn)行有效的估計(jì)。因此，

相對(duì)于變化的、不確定的測(cè)量噪聲，本文研究的基于自適應(yīng) EKF相對(duì)導(dǎo)航算法具有較強(qiáng)的魯棒性并可獲得高精度的相對(duì)定位參數(shù)。

[1] 范科, 趙偉, 劉建業(yè). 自適應(yīng)濾波算法在 SINS/GPS組合導(dǎo)航系統(tǒng)中的應(yīng)用研究[J]. 航空電子技術(shù), 2008,39(3), 11-15.

[2] 田海, 朱新巖. 一種簡(jiǎn)化的 Sage_Husa卡爾曼濾波[J].彈箭與制導(dǎo)學(xué)報(bào),2011, 31(1), 75-77.

[3] 梁松, 徐曉蘇, 黃永亮. Sage_Husa自適應(yīng)濾波算法在捷聯(lián)組合導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 測(cè)試技術(shù)學(xué)報(bào), 2011,25(4), 327-331.