朱 敏，馬 振，王俊瑋，陳熙源

(東南大學(xué)儀器科學(xué)與工程學(xué)院， 南京 210096)

0 引言

分布式位姿測量系統(tǒng)可為高分辨率航空對地觀測系統(tǒng)多元載荷提供分布式高精度運(yùn)動信息。但觀測平臺在飛行過程中受到載荷和湍流的影響，機(jī)翼發(fā)生撓曲變形和震顫，降低了主子節(jié)點(diǎn)之間傳遞對準(zhǔn)的精度[1-2]。因此，提高分布式傳遞對準(zhǔn)的精度成為航空對地觀測系統(tǒng)高分辨率成像的關(guān)鍵。

傳統(tǒng)的不考慮主子節(jié)點(diǎn)動態(tài)變形的傳遞對準(zhǔn)誤差模型以及不考慮噪聲的時變特性和主子節(jié)點(diǎn)間信息融合的分布式卡爾曼濾波算法用于分布式傳遞對準(zhǔn)時難以提供高精度信息。傳統(tǒng)傳遞對準(zhǔn)誤差模型將機(jī)翼視為剛體，忽略了主子節(jié)點(diǎn)之間的動態(tài)變形，難以實(shí)現(xiàn)高精度測量。文獻(xiàn)[3-4]使用二階Markov模擬動態(tài)彎曲變形角，并將相關(guān)彎曲變形角和角速率加入狀態(tài)變量，但其模型中將動態(tài)杠桿臂視為常數(shù)，并不符合實(shí)際變形。文獻(xiàn)[5]建立了變形角和動態(tài)桿臂之間的模型，對主子節(jié)點(diǎn)之間的速度關(guān)系進(jìn)行包括耦合關(guān)系在內(nèi)的詳細(xì)分析。但在機(jī)載分布式傳遞對準(zhǔn)過程中，機(jī)翼變形會增加桿臂的估計(jì)誤差，姿態(tài)以及角速度匹配方法可避免該誤差[6]。所以，主子節(jié)點(diǎn)之間角速度耦合關(guān)系，即機(jī)翼撓曲變形引起的角速度同子節(jié)點(diǎn)角速度方向不同而存在的耦合角也需要考慮。因此，在傳遞對準(zhǔn)誤差模型建立時考慮了機(jī)翼的撓曲變形角、耦合角、主子節(jié)點(diǎn)的動態(tài)桿臂誤差。但在實(shí)際的工程應(yīng)用中，盡管使用了較為精確的模型，復(fù)雜飛行環(huán)境的干擾仍會導(dǎo)致卡爾曼濾波精度下降，甚至濾波發(fā)散，自適應(yīng)濾波方法成為研究重點(diǎn)[7-9]。文獻(xiàn)[10]提出了基于新息極大似然估計(jì)準(zhǔn)則的自適應(yīng)卡爾曼濾波器,實(shí)時估計(jì)出系統(tǒng)過程噪聲矩陣Q和量測噪聲矩陣R，可以較好估計(jì)出失準(zhǔn)角，但每一次均要進(jìn)行Q和R的估計(jì)，大的計(jì)算量會導(dǎo)致實(shí)時性降低。因此，考慮到傳遞對準(zhǔn)的精度和時間，采用只對R進(jìn)行不斷估計(jì)的方法，但狀態(tài)估計(jì)誤差會影響該濾波算法的穩(wěn)定性?；跉埐畹目ǚ綑z驗(yàn)[11]可有效解決該問題。因此，本文引入R更新和基于卡方檢驗(yàn)的協(xié)方差矩陣P更新的卡爾曼濾波算法進(jìn)行單點(diǎn)傳遞對準(zhǔn)。但單一的自適應(yīng)卡爾曼濾波用于分布式傳遞對準(zhǔn)時抗擾動性能較差，可將不同的子濾波器進(jìn)行信息融合以達(dá)到更好的效果[12]。在多節(jié)點(diǎn)信息融合算法中，聯(lián)邦濾波因其較好的容錯性而被廣泛應(yīng)用，其通過實(shí)時確定信息權(quán)重系數(shù)的方式對聯(lián)邦濾波器進(jìn)行了改進(jìn)[13-14]。因此，本文結(jié)合分布式傳遞對準(zhǔn)的背景和復(fù)雜飛行環(huán)境，以提高傳遞對準(zhǔn)的精度和穩(wěn)定性、縮短傳遞對準(zhǔn)的時間為目標(biāo)，提出了新型聯(lián)邦自適應(yīng)濾波算法用于分布式傳遞對準(zhǔn)，子濾波器采用基于R和P自適應(yīng)更新的卡爾曼濾波(Adaptive Kalman Filter，AKF)并結(jié)合27維傳遞對準(zhǔn)模型進(jìn)行單點(diǎn)傳遞對準(zhǔn)，主濾波器采用基于誤差協(xié)方差矩陣的范數(shù)的原則對多個子濾波器進(jìn)行信息融合。

1 新型聯(lián)邦自適應(yīng)濾波算法

1.1 分布式位姿測量系統(tǒng)

分布式位姿測量系統(tǒng)包括一個高精度的主IMU和多個子IMU，示意圖如圖1所示。由于飛機(jī)的機(jī)翼部分可承受質(zhì)量有一定限制，所以機(jī)翼上安裝的多個子IMU為低精度。各個子節(jié)點(diǎn)處高精度運(yùn)動信息的獲取需利用主IMU測得的高精度運(yùn)動信息對分布于機(jī)翼的多個子IMU進(jìn)行分布式傳遞對準(zhǔn)濾波估計(jì)[15]。分布式傳遞對準(zhǔn)精度取決于傳遞對準(zhǔn)模型和分布式傳遞對準(zhǔn)濾波方法。

圖1 分布式位姿測量系統(tǒng)示意圖Fig.1 Schematic diagram of distributed position and orientation system

本文采用27維狀態(tài)量的傳遞對準(zhǔn)模型，將機(jī)翼的撓曲變形角、耦合角、主子節(jié)點(diǎn)的動態(tài)桿臂誤差均進(jìn)行考慮，模型[16]如下：

狀態(tài)方程中狀態(tài)變量為

(1)

狀態(tài)方程中狀態(tài)轉(zhuǎn)移矩陣為

(2)

量測方程中量測量包括主子節(jié)點(diǎn)之間的姿態(tài)差Ya、速度差Yv和角速度差Yω

(3)

量測方程中量測矩陣為

(4)

(5)

(6)

傳遞對準(zhǔn)過程中不確定的模型以及時變的飛行環(huán)境噪聲等會帶來誤差。不考慮噪聲時變特性和主子節(jié)點(diǎn)信息融合的分布式卡爾曼濾波算法用于分布式傳遞對準(zhǔn)時難以提供高精度信息。為了保證傳遞對準(zhǔn)精度和穩(wěn)定性，將新型聯(lián)邦自適應(yīng)濾波算法用于分布式傳遞對準(zhǔn)。子濾波器中使用基于R和P自適應(yīng)更新的卡爾曼濾波算法，主濾波器基于誤差協(xié)方差矩陣進(jìn)行信息融合。

1.2 自適應(yīng)卡爾曼濾波算法

傳遞對準(zhǔn)時系統(tǒng)模型會產(chǎn)生不可預(yù)知的變化，需不斷地估計(jì)Q和R來提高精度，但其存在著計(jì)算量大和自適應(yīng)性差的問題。每一次濾波均要進(jìn)行Q和R的估計(jì)，計(jì)算量大，實(shí)時性降低，并且隨著濾波遞推次數(shù)的增加,分配權(quán)重不斷減小，濾波自適應(yīng)性降低[14]。為保證濾波精度和速度，采用基于R和P自適應(yīng)更新的卡爾曼濾波算法。

針對實(shí)時性問題，在Sage-Husa自適應(yīng)卡爾曼濾波的基礎(chǔ)上選擇只進(jìn)行量測噪聲R更新的卡爾曼濾波算法。傳遞對準(zhǔn)中主慣導(dǎo)采用高精度的IMU，同時還有相應(yīng)的輔助導(dǎo)航設(shè)備以修正主慣導(dǎo)誤差，常認(rèn)為主慣導(dǎo)輸出信息的精度較高并且較穩(wěn)定。但是由于在實(shí)際傳遞對準(zhǔn)中，受機(jī)翼撓曲變形和振顫的影響，機(jī)體運(yùn)動速度、姿態(tài)以及角速度會發(fā)生變化，且變化較為復(fù)雜，雖在傳遞對準(zhǔn)誤差建模中已針對機(jī)翼撓曲變形進(jìn)行建模，但其并未完全消除機(jī)翼變形帶來的影響。相對于量測噪聲而言，系統(tǒng)過程噪聲受子慣導(dǎo)慣性器件誤差的影響較小。因此，為減少濾波器的計(jì)算量并增加其穩(wěn)定性，本文采用只進(jìn)行量測噪聲R更新的卡爾曼濾波算法，將量測噪聲R不斷估計(jì)并傳入，進(jìn)行濾波增益的更新，從而達(dá)到更精確的狀態(tài)估計(jì)，具體算法流程如圖2所示。

圖2 R更新的卡爾曼濾波算法Fig.2 Kalman filtering algorithm based on R-update

R更新方程中自適應(yīng)系數(shù)dk和遺忘因子[14]關(guān)系如下

dk=(1-a)/(1-ak)

(7)

其中，遺忘因子a的取值范圍為0.95～0.99。

針對濾波的自適應(yīng)問題，上述只進(jìn)行R更新的卡爾曼濾波算法，在R更新方程中遞推次數(shù)到一定值后，自適應(yīng)系數(shù)收斂為常數(shù)，歷史和新近的觀測信息對濾波的影響保持固定比例，歷史觀測信息對濾波影響較大，濾波的自適應(yīng)性較差。所以，濾波過程中針對濾波遞推次數(shù)進(jìn)行相應(yīng)調(diào)整，每次濾波結(jié)束后判斷自適應(yīng)是否為常數(shù)，若為常數(shù)，將濾波次數(shù)k從0開始計(jì)數(shù)，重新進(jìn)行自適應(yīng)調(diào)整，繼續(xù)執(zhí)行圖2所示濾波過程。

然而，系統(tǒng)模型的不確定性會降低基于R自適應(yīng)更新的卡爾曼濾波器的性能，只進(jìn)行R更新的卡爾曼濾波算法易受到狀態(tài)估計(jì)誤差的影響，計(jì)算穩(wěn)定性相對較差。為克服該問題，本文引入卡方檢驗(yàn)以及自適應(yīng)因子[17]。通過基于殘差序列的卡方檢驗(yàn)評估傳遞對準(zhǔn)模型的濾波穩(wěn)定性，并采用自適應(yīng)因子動態(tài)調(diào)整狀態(tài)估計(jì)誤差的均方誤差陣，具體如下：

通過卡方檢驗(yàn)驗(yàn)證濾波器是否平穩(wěn)，當(dāng)檢測到濾波不是最優(yōu)時，P陣進(jìn)行自適應(yīng)更新，引入自適應(yīng)因子調(diào)整狀態(tài)協(xié)方差矩陣并考慮對稱性，P的表達(dá)式更新為式(8)

(8)

其中，Sk為自適應(yīng)因子。

當(dāng)濾波最優(yōu)時，殘差序列rk有下列屬性

(9)

其中，γk服從可觀測狀態(tài)變量的數(shù)量為m自由度的卡方分布。

式(9)中，令

(10)

(11)

(12)

根據(jù)式(9)可知，濾波最優(yōu)時殘差序列的每一個分量滿足下列方程

γi(k)=[ri(k)]2/(aii(k)+bii(k))～χ(1)

(13)

其中，aii(k)為矩陣Ak的第i個對角線元素；bii(k) 為矩陣Bk第i個對角線元素。

對Ak進(jìn)行分解可得到Ak和Jk的關(guān)系

(14)

其中，Jii(k)為矩陣Jk的第i個對角線元素。

結(jié)合式(13)和式(14)，自適應(yīng)因子滿足下列方程

(15)

其中，εi為卡方檢驗(yàn)的閾值。

檢測到濾波異常時，為使濾波正常，自適應(yīng)因子公式如下

Si=

(16)

自適應(yīng)因子矩陣中只有可觀測狀態(tài)量可以自適應(yīng)計(jì)算，其余不可觀測的值均取為1。因此，可觀測狀態(tài)量的自適應(yīng)因子取值如式(16)，不可觀測狀態(tài)量取值為1。

自適應(yīng)因子矩陣表達(dá)式如下

Sk=diag(s1,s2,s3,…,sn)

(17)

綜上所述，基于R和P自適應(yīng)更新的卡爾曼濾波算法整體流程如圖3所示。

圖3 自適應(yīng)卡爾曼濾波算法Fig.3 Adaptive Kalman filtering algorithm

1.3 基于誤差協(xié)方差矩陣的信息融合算法

為提高分布式傳遞對準(zhǔn)精度和穩(wěn)定性，采用基于誤差協(xié)方差矩陣的信息融合算法。結(jié)合子濾波器中的自適應(yīng)卡爾曼濾波算法，以3個子IMU為例的新型聯(lián)邦自適應(yīng)濾波算法的濾波結(jié)構(gòu)如圖4所示。

圖4 新型聯(lián)邦自適應(yīng)濾波算法Fig.4 Novel federated adaptive filtering algorithm

子濾波器中分別進(jìn)行基于R和P自適應(yīng)更新的卡爾曼濾波算法的傳遞對準(zhǔn)后，在主濾波器中通過信息融合進(jìn)行全局最優(yōu)估計(jì)，然后將計(jì)算出的協(xié)方差陣乘以信息分配因子系數(shù),反饋給各個子濾波器進(jìn)行權(quán)重的分配。信息分配因子αj為權(quán)重分配提供基準(zhǔn)，其可通過誤差協(xié)方差矩陣P的范數(shù)被確定。子濾波器的濾波效果表達(dá)式如下

(18)

其中，j=1,2,3代表濾波器的序號。

信息權(quán)重因子αj的表達(dá)式如下

(19)

信息融合原則為

(20)

信息權(quán)重分配原則為

(21)

2 實(shí)驗(yàn)結(jié)果及分析

2.1 半物理仿真實(shí)驗(yàn)

為了驗(yàn)證基于27維傳遞對準(zhǔn)模型的新型聯(lián)邦自適應(yīng)濾波算法的有效性，本文采用基于實(shí)際飛行數(shù)據(jù)的半物理仿真方法，使用Y12型飛機(jī)進(jìn)行約200min、6000m高的飛行實(shí)驗(yàn)，并記錄飛行數(shù)據(jù)。其飛行軌跡如圖5所示。

圖5 飛行軌跡Fig.5 Flight trajectory

實(shí)驗(yàn)所使用的仿真數(shù)據(jù)基于上述實(shí)際飛行軌跡生成。其中主IMU數(shù)據(jù)采用本次飛行實(shí)驗(yàn)數(shù)據(jù)，3個子IMU數(shù)據(jù)將飛行實(shí)驗(yàn)中主IMU數(shù)據(jù)進(jìn)行轉(zhuǎn)換，再疊加設(shè)定的子 IMU的誤差。參數(shù)設(shè)置如表1所示。

表1 子IMU參數(shù)設(shè)置

2.2 結(jié)果及分析

3個子IMU分別采用傳統(tǒng)卡爾曼濾波算法、聯(lián)邦自適應(yīng)濾波算法、新型聯(lián)邦自適應(yīng)濾波算法三種算法進(jìn)行分布式傳遞對準(zhǔn),以證明聯(lián)邦自適應(yīng)濾波算法以及自適應(yīng)卡爾曼濾波算法的有效性，最終證明本文提出的新型聯(lián)邦自適應(yīng)濾波算法的有效性。三種濾波算法均采用27維傳遞對準(zhǔn)模型，選用桿臂誤差和姿態(tài)誤差作為評估對象。傳統(tǒng)卡爾曼濾波算法和聯(lián)邦自適應(yīng)濾波算法對比仿真實(shí)驗(yàn)結(jié)果如圖6和圖7所示。

圖6 桿臂估計(jì)誤差對比圖Fig.6 Comparison of lever arm estimation error

圖7 姿態(tài)估計(jì)誤差對比圖Fig.7 Comparison of attitude estimation error

傳統(tǒng)卡爾曼濾波算法和新型聯(lián)邦自適應(yīng)濾波算法對比仿真實(shí)驗(yàn)結(jié)果如圖8和圖9所示。

圖8 桿臂估計(jì)誤差對比圖Fig.8 Comparison of lever arm estimation error

圖9 姿態(tài)估計(jì)誤差對比圖Fig.9 Comparison of attitude estimation error

將圖8、圖9和圖6、圖7進(jìn)行對比可以看出，相較于傳統(tǒng)卡爾曼濾波算法以及子濾波器使用卡爾曼濾波的聯(lián)邦自適應(yīng)濾波算法，新型聯(lián)邦自適應(yīng)濾波算法可提高動態(tài)桿臂以及姿態(tài)估計(jì)的穩(wěn)定性和精度。在卡爾曼濾波算法下，動態(tài)桿臂估計(jì)精度較差，姿態(tài)角估計(jì)穩(wěn)定性較差，濾波器處于振蕩效果，并且航向角的估計(jì)精度較差。因此，算法效果主要以穩(wěn)定性和動態(tài)桿臂、航向角的估計(jì)精度為評判標(biāo)準(zhǔn)。由于飛行過程中機(jī)翼振動和撓曲變形會導(dǎo)致量測信息噪聲時變，飛行環(huán)境的多變性則會導(dǎo)致受擾動較大。若使用傳統(tǒng)卡爾曼濾波方法無法有效進(jìn)行桿臂和姿態(tài)的校正，誤差迅速增大，降低了傳遞對準(zhǔn)的穩(wěn)定性、快速性和精度。雖然使用聯(lián)邦自適應(yīng)濾波算法可提高桿臂和姿態(tài)的估計(jì)精度，增加整體的抗擾動性能，但子濾波器估計(jì)的信息振蕩嚴(yán)重、收斂緩慢，且未考慮系統(tǒng)噪聲和量測噪聲的影響，傳入主濾波器后降低了信息融合的效果。本文提出的新型聯(lián)邦自適應(yīng)濾波算法有效解決了上述問題。子濾波器中R自適應(yīng)更新的加入，對時變噪聲有一定調(diào)節(jié)能力，可以正確估計(jì)Rk，實(shí)時調(diào)整增益K，P自適應(yīng)更新的加入，對系統(tǒng)噪聲特性的影響進(jìn)行削減，同時主濾波器進(jìn)行信息融合，促進(jìn)了整體濾波器的抗擾動性能，增強(qiáng)了校正效果。新型聯(lián)邦自適應(yīng)濾波算法縮短了傳遞對準(zhǔn)的時間，提高了傳遞對準(zhǔn)的精度，增強(qiáng)了傳遞對準(zhǔn)的穩(wěn)定性。

為直觀分析提出方法的有效性，三種算法的動態(tài)桿臂及航向角估計(jì)的均方根誤差(Root Mean Square Error，RMSE)計(jì)算如表2和表3所示。選用RMSE作為評估標(biāo)準(zhǔn)，相較于傳統(tǒng)的卡爾曼濾波算法，新型聯(lián)邦自適應(yīng)濾波算法用于分布式傳遞對準(zhǔn)時動態(tài)桿臂估計(jì)精度提高了61.54%，航向角估計(jì)精度提高了42.35%。

表2 桿臂估計(jì)誤差結(jié)果對比

表3 航向角估計(jì)誤差結(jié)果對比

3 結(jié)論

針對分布式傳遞對準(zhǔn)精度下降和穩(wěn)定性問題，提出了子濾波器采用基于R和P自適應(yīng)更新的卡爾曼濾波算法并結(jié)合27維傳遞對準(zhǔn)模型用于主子傳遞對準(zhǔn)，然后對不同子濾波器采用基于誤差協(xié)方差矩陣原則進(jìn)行信息融合和權(quán)重分配。算法分析和實(shí)驗(yàn)結(jié)果表明：新型聯(lián)邦自適應(yīng)濾波算法中R自適應(yīng)、P自適應(yīng)以及信息融合方法的加入，抑制了量測信息噪聲時變特性對子濾波器的影響，緩解了狀態(tài)估計(jì)誤差對濾波的影響，提高了抗擾動性能，動態(tài)桿臂和姿態(tài)估計(jì)穩(wěn)定性和精度得到了提高，證明了新型聯(lián)邦自適應(yīng)濾波算法應(yīng)用于分布式傳遞對準(zhǔn)中的有效性。未來計(jì)劃建立更精確的傳遞對準(zhǔn)建模，將陀螺儀和加速度計(jì)進(jìn)行更深層次的展開和分析以提高精度。