HCKF性能評估及其在傳遞對準(zhǔn)中的應(yīng)用

蘇炳志，穆榮軍，龍 騰，程 超，崔乃剛

(1. 哈爾濱工業(yè)大學(xué)航天學(xué)院，哈爾濱 150001；2. 北京機電工程研究所，北京 100074)

0 引 言

傳遞對準(zhǔn)是指在自然擾動或戰(zhàn)術(shù)機動的動態(tài)環(huán)境下，利用已處于導(dǎo)航狀態(tài)的高精度主慣導(dǎo)系統(tǒng)對子慣導(dǎo)系統(tǒng)進行初始對準(zhǔn)的一種方法。1989年美國學(xué)者Kain等首次提出基于“速度+姿態(tài)”匹配的快速傳遞對準(zhǔn)方法[1]，提高了對準(zhǔn)收斂速度和減小了對運動平臺的機動要求，在快速響應(yīng)的艦載機和艦載武器系統(tǒng)中得到廣泛的應(yīng)用。艦載機著陸在艦船甲板上的位置是任意的，而艦船主慣導(dǎo)系統(tǒng)卻安裝在甲板下方的導(dǎo)航室內(nèi)；此時，主子慣導(dǎo)之間的失準(zhǔn)角可能很大，需要建立大失準(zhǔn)角傳遞對準(zhǔn)模型[2-3]。

基于貝葉斯框架的非線性濾波和估計為大失準(zhǔn)角傳遞對準(zhǔn)系統(tǒng)提供了最優(yōu)的解決方案，特別是利用不同采樣策略對非線性高斯系統(tǒng)狀態(tài)的后驗均值和方差進行近似的高斯濾波。其中，UKF(Unscented Kalman filter)通過使用一組sigma點經(jīng)非線性函數(shù)傳遞后的加權(quán)和來逼近狀態(tài)的概率密度函數(shù)，但只有3階精度[4-6]；為了提高估計精度，Arasaratnam等[7]基于高斯厄米特積分(Gaussian-Hermite quadrature,GHQ)提出了GHQF(Gaussian-Hermite quadrature filter)，能夠達到任意階的估計精度，但由于多維GHQF是由單維GHQ準(zhǔn)則通過張量積直接擴展得到的，因此GHQF的積分點隨著狀態(tài)維數(shù)的增加呈指數(shù)增長，即“維數(shù)災(zāi)難”；針對高維狀態(tài)估計，Arasaratnam等[8]基于3階球面-徑向容積準(zhǔn)則，提出了容積卡爾曼濾波(Cubature Kalman filter, CKF)，文獻[9-10]基于Genz積分和矩匹配法推導(dǎo)了任意階的球面積分和徑向積分，從而建立了高階容積卡爾曼濾波。HCKF(High-degree cubature Kalman filter)作為一種新興的濾波算法被運用于對準(zhǔn)系統(tǒng)中[11-12]。

針對非線性濾波算法的性能評估，國內(nèi)外學(xué)者的研究重點主要在濾波估計精度分析[13-16]，較少有分析算法計算量的。運用泰勒級數(shù)展開精度分析法：文獻[13-14]分析了UKF算法的估計精度，文獻[15]比較分析了UKF和CKF的估計精度，文獻[16]對UKF、CKF和GHQF三種濾波算法的精度進行了比較分析。文獻[17]運用計算復(fù)雜度分析了傳遞對準(zhǔn)濾波器的計算量，但僅適用于標(biāo)準(zhǔn)卡爾曼濾波。

本文以艦機大失準(zhǔn)角傳遞對準(zhǔn)為應(yīng)用背景，建立非線性快速傳遞對準(zhǔn)模型；基于高階球面-徑向容積準(zhǔn)則建立HCKF算法模型；綜合運用多變量函數(shù)泰勒級數(shù)展開和計算復(fù)雜度分析對HCKF的估計精度和計算量予以分析，并與GHQF和UKF進行比較，綜合評估濾波算法性能；參照GHQF和UKF，分別通過艦機大失準(zhǔn)角傳遞對準(zhǔn)搖擺臺試驗中的對準(zhǔn)精度統(tǒng)計和計算耗時加以驗證，從而綜合驗證HCKF性能評估方法的有效性。

1 非線性快速傳遞對準(zhǔn)模型

1.1 捷聯(lián)式慣導(dǎo)系統(tǒng)誤差傳播模型

以“北-天-東”地理系為導(dǎo)航系，大失準(zhǔn)角下的非線性快速傳遞對準(zhǔn)誤差模型為[1,18]：

(1)

(2)

(3)

1.2 非線性快速傳遞對準(zhǔn)模型

考慮到對準(zhǔn)時間短，對陀螺儀零偏和加速度計零偏沒有估計效果，且高精度的器件誤差對對準(zhǔn)精度影響小，為了減小計算量，提高運行速度，因此未將其列入傳遞對準(zhǔn)狀態(tài)向量中；鑒于本文的研究重點是進行HCKF算法的性能評估，而非撓曲變形建模，因此在初始對準(zhǔn)過程中，認(rèn)為主、子慣導(dǎo)之間的位置是固定不變的。根據(jù)式(1)，構(gòu)建9維非線性快速傳遞對準(zhǔn)模型，采用“速度+姿態(tài)”匹配實現(xiàn)艦機之間的快速傳遞對準(zhǔn)。

1.2.1非線性快速傳遞對準(zhǔn)狀態(tài)模型

選擇量測失準(zhǔn)角矢量ψm，速度誤差矢量δV和安裝偏差角矢量ψa為傳遞對準(zhǔn)狀態(tài)向量，即：

(4)

根據(jù)式(1)可得到狀態(tài)模型微分方程：

(5)

1.2.2非線性快速傳遞對準(zhǔn)量測模型

選擇速度誤差矢量δV和量測失準(zhǔn)角矢量ψm為傳遞對準(zhǔn)量測向量，即：

(6)

式中：Vm,Vs分別為主慣導(dǎo)和子慣導(dǎo)的速度矢量，g(·)表示從方向余弦矩陣求取歐拉角的函數(shù)，矩陣Cz的定義如下：

(7)

式(6)中：

2 高階容積卡爾曼濾波算法

2.1 高階球面-徑向容積準(zhǔn)則

考慮如下非線性離散系統(tǒng)狀態(tài)空間模型：

(8)

其中，xk∈Rn是狀態(tài)向量，zk∈Rm是量測向量，f(·)和h(·)是非線性向量函數(shù)，可分別由非線性快速傳遞對準(zhǔn)狀態(tài)模型和量測模型求得，wk-1和vk分別為過程和量測噪聲，均方誤差矩陣分別為Qk-1和Rk。

(9)

(10)

2.2 高階容積卡爾曼濾波算法

根據(jù)式(8)和(9)并結(jié)合非線性高斯濾波遞推公式[8]可得到高階(5階)容積卡爾曼濾波的具體計算步驟：

1) 時間更新

計算k-1時刻的容積點

(11)

其中，n是狀態(tài)向量維數(shù)，向量為：

χi=

(12)

一步預(yù)測：

(13)

式中權(quán)值wi為：

(14)

2) 量測更新

計算量測更新容積點：

(15)

經(jīng)量測非線性變換的容積點：

(16)

計算濾波增益：

(17)

狀態(tài)估計值及協(xié)方差陣計算：

(18)

3 高階容積卡爾曼濾波算法性能評估

性能評估由算法的估計精度和計算量分析兩部分組成。分別以濾波算法的泰勒級數(shù)展開精度和計算復(fù)雜度評估HCKF估計精度和計算量，并與GHQF和UKF進行比較。

3.1 高階容積卡爾曼濾波算法精度分析

3.1.1多變量函數(shù)泰勒級數(shù)展開

(19)

3.1.2高階容積卡爾曼濾波估計精度

(20)

其中，

δxi=

(21)

根據(jù)式(19)，并考慮到2n2+1個HCKF算法的容積點是對稱的，奇數(shù)階矩相互抵消，則映射后f(x)的均值為：

(22)

其中，二階矩計算過程為：

(23)

若考慮其他偶數(shù)階泰勒級數(shù)展開項，i=1,2,…,2n時

(24)

同理，i=2n+1,2n+2,…,2n2時

(25)

根據(jù)式(24)和(25)可得：

(26)

由式(23)和(26)可得：

(27)

3.1.3GHQF和UKF估計精度

(28)

(29)

3.1.4估計精度比較

將式(27)～(29)分別與多變量函數(shù)泰勒級數(shù)展開式(19)比較可知，高階(5階)容積卡爾曼濾波能精確傳播至5階項，從6階項開始出現(xiàn)截斷誤差，即有5階泰勒級數(shù)展開精度；GHQF的估計精度由單維高斯厄米特積分點數(shù)q決定，qn個積分點的多維高斯厄米特積分濾波能夠精確傳播至2q-1階項，從2q階項開始出現(xiàn)截斷誤差。UKF能精確傳播至3階項，從4階項開始出現(xiàn)截斷誤差，即有3階泰勒級數(shù)展開精度。

3.2 高階容積卡爾曼濾波計算量分析

對于一種濾波算法不僅需要考慮它的估計精度，而且還必須考慮它的計算量。本節(jié)將建立計算復(fù)雜度模型來評估HCKF算法的計算量，并與GHQF和UKF進行比較分析。

3.2.1三種濾波采樣點數(shù)

HCKF、GHQF和UKF均屬于確定性采樣濾波方法[19]，下面將容積點、積分點和sigma點統(tǒng)稱為采樣點，三種濾波算法的采樣點數(shù)與狀態(tài)向量維數(shù)n的關(guān)系如式(30)和表1所示：

(30)

其中，為了更好地比較分析HCKF算法的性能，GHQF的估計精度也取5階，即q=3。

表1 HCKF、GHQF和UKF采樣點數(shù)Table 1 Number of sampling points for HCKF,GHQF and UKF

3.2.2計算復(fù)雜度建模

求取k-1時刻容積點式(11)的計算復(fù)雜度

(31)

式中：Tm1(n)表示乘法個數(shù)，Ta1(n)表示加法個數(shù)，Cholesky分解(平方根法)的計算復(fù)雜度n3/3按加法計算。

一步預(yù)測式(13)的計算復(fù)雜度

(32)

式中：Mx和Ax分別表示非線性傳遞函數(shù)f(x)的乘法和加法運算個數(shù)，Nm為采樣點數(shù)。

求取量測更新容積點式(15)的計算復(fù)雜度

(33)

求取經(jīng)量測非線性變換容積點式(16)的計算復(fù)雜度

(34)

式中：Mz和Az分別表示非線性量測函數(shù)h(x)的乘法和加法運算個數(shù)。

濾波增益計算式(17)的計算復(fù)雜度

(35)

狀態(tài)估計及方差矩陣更新式(18)的計算復(fù)雜度

(36)

由式(31)～(36)，算法的計算復(fù)雜度T(n)為：

(n2+nm+m2+n+3m)Nm+

(Mx+Mz)Nm+n2m+2n2+2nm2+nm

(37)

(n2+nm+m2+4n+4m)Nm+

(Ax+Az)Nm+m3+2/3n3+n2m+

nm2+5n2+m2-3nm-2n

(38)

T(n)=Tm(n)+Ta(m)

(39)

式中：Tm(n)表示乘法個數(shù)，Ta(n)表示加法個數(shù)，T(n)表示算法的計算復(fù)雜度。

3.2.3算法計算量分析

量測向量維數(shù)m一般小于狀態(tài)向量維數(shù)n，因此HCKF的計算復(fù)雜度與狀態(tài)維數(shù)n的四次方成正相關(guān)，即T(n)=O(n4)；而GHQF的計算復(fù)雜度隨著狀態(tài)維數(shù)n的增加呈指數(shù)增長趨勢，即T(n)=O(n2×3n)[20]；UKF的計算復(fù)雜度與狀態(tài)維數(shù)n的三次方成正相關(guān)，即T(n)=O(n3)[20]。

由于計算復(fù)雜度越大，算法的計算量越大，因此根據(jù)上述理論分析可知，在相同精度等級條件下HCKF的計算量比GHQF小。

3.3 性能評估在大失準(zhǔn)角傳遞對準(zhǔn)中的應(yīng)用

將基于多變量函數(shù)泰勒級數(shù)展開和計算復(fù)雜度分析的性能評估方法運用到大失準(zhǔn)角傳遞對準(zhǔn)中。

根據(jù)第3.1.4節(jié)估計精度比較可知，HCKF與GHQF都具有5階泰勒級數(shù)展開精度，高于UKF的3階泰勒級數(shù)展開精度，在艦機大失準(zhǔn)角傳遞對準(zhǔn)等強非線性系統(tǒng)中更加能突顯HCKF的精度優(yōu)勢。根據(jù)第3.2.3節(jié)算法計算量分析可知，對于大失準(zhǔn)角傳遞對準(zhǔn)模型(n=9)，此時HCKF的計算量比UKF的高1個數(shù)量級，但比GHQF的小2個數(shù)量級。

綜合上述分析，HCKF能夠有效提高艦機大失準(zhǔn)角傳遞對準(zhǔn)精度，同時又不會致使計算量急劇增大，綜合性能更優(yōu)，更加有望運用于工程實際中。

4 試驗校驗

為了校驗高階(5階)容積卡爾曼濾波性能評估的理論分析結(jié)果，開展了快速傳遞對準(zhǔn)搖擺臺試驗。為了盡量排除干擾，在相對比較理想的試驗條件下進行試驗驗證，能更加精準(zhǔn)的評估HCKF算法性能，因此試驗主子慣導(dǎo)之間是剛性連接的。利用六自由度搖擺臺模擬中等海況[3]：載體的俯仰角?，橫滾角γ，偏航角ψ的幅值和周期分別為5°和8 s，橫蕩、縱蕩和垂蕩引起的線速度幅值和周期分別為0.1 m和10 s。搖擺臺試驗系統(tǒng)組成及主子慣導(dǎo)器件指標(biāo)如圖1所示。

圖1 快速傳遞對準(zhǔn)搖擺臺試驗系統(tǒng)結(jié)構(gòu)Fig.1 The rapid transfer alignment swing bench test system

為了全面考核與驗證算法對安裝偏差角估計的正確性，通過三軸電控角位移臺改變主子慣導(dǎo)之間的安裝偏差角，水平安裝偏差角ψax=3°，ψaz=3°，航向安裝偏差角可取ψay=±10°,±20°, ±30°,±40°,±50°，共10個試驗工況。

基于三種非線性濾波算法的快速傳遞對準(zhǔn)搖擺臺試驗安裝偏差角估計誤差曲線(航向安裝偏差角20°工況)如圖2～圖4所示，10次搖擺臺試驗對準(zhǔn)精度統(tǒng)計結(jié)果(安裝偏差角估計值與光學(xué)瞄準(zhǔn)獲得的真值相比)如表2所示，三種濾波算法單次計算耗時(硬件配置：Core i7-4790 CPU，軟件配置：Visual Studio 2005)如表3所示。

圖3 Y軸安裝偏差角估計誤差Fig.3 Estimation error of installation deviation angle of Y axis

圖4 Z軸安裝偏差角估計誤差Fig.4 Estimation error of installation deviation angle of Z axis

由圖2～4和表2～3三種濾波算法在傳遞對準(zhǔn)中的試驗結(jié)果可知，從對準(zhǔn)精度角度分析HCKF和GHQF的估計精度相當(dāng)，比UKF高17%。從計算耗時角度分析，HCKF單次計算耗時為0.187 ms稍高于UKF的0.024 ms，但比GHQF的23.83 ms小2個數(shù)量級；UKF和HCKF計算耗時小于慣導(dǎo)的更新周期(10 ms)，而GHQF的計算耗時大于慣導(dǎo)的更新周期，不能滿足實時計算需求。HCKF在有效提高對準(zhǔn)精度的同時又能保證系統(tǒng)的實時性，綜合性能更優(yōu)，具有更加廣闊的運用前景；傳遞對準(zhǔn)精度統(tǒng)計和計算耗時結(jié)果與HCKF算法精度和計算量分析結(jié)果一致，校驗了基于多變量函數(shù)泰勒級數(shù)展開和計算復(fù)雜度分析的性能評估方法的有效性。

表2 10次傳遞對準(zhǔn)試驗RMSE統(tǒng)計結(jié)果Table 2 Statistics results of transfer alignmentRMSE in 10 tests

表3 三種濾波算法計算耗時比較Table 3 Comparison of filtering computation times

5 結(jié) 論

本文以艦機大失準(zhǔn)角傳遞對準(zhǔn)為應(yīng)用背景，建立了非線性快速傳遞對準(zhǔn)模型；基于高階球面-徑向容積準(zhǔn)則建立了HCKF算法模型，利用多變量泰勒級數(shù)展開和計算復(fù)雜度分析衡量HCKF的估計精度和計算量，從而綜合評估HCKF的性能，并與GHQF和UKF進行比較。理論分析表明：HCKF和GHQF均具有5階泰勒級數(shù)展開精度，高于UKF的3階精度；HCKF的計算復(fù)雜度O(n4)比UKF的O(n3)高一個數(shù)量級(n=9)，但比GHQF的O(n2×3n)小2個數(shù)量級。艦機大失準(zhǔn)角傳遞對準(zhǔn)搖擺臺試驗校驗表明：HCKF的估計精度與GHQF相當(dāng)，比UKF高17%，計算量稍高于UKF，但比GHQF小2個數(shù)量級，滿足實時計算需求；HCKF能夠兼顧估計精度與計算量，綜合性能更優(yōu)，運用前景更加廣闊；試驗結(jié)果與HCKF的性能評估理論分析結(jié)果一致，校驗了性能評估算法的有效性。