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

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

0 引 言

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

基于貝葉斯框架的非線性濾波和估計為大失準角傳遞對準系統(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準則通過張量積直接擴展得到的，因此GHQF的積分點隨著狀態(tài)維數(shù)的增加呈指數(shù)增長，即“維數(shù)災難”；針對高維狀態(tài)估計，Arasaratnam等[8]基于3階球面-徑向容積準則，提出了容積卡爾曼濾波(Cubature Kalman filter, CKF)，文獻[9-10]基于Genz積分和矩匹配法推導了任意階的球面積分和徑向積分，從而建立了高階容積卡爾曼濾波。HCKF(High-degree cubature Kalman filter)作為一種新興的濾波算法被運用于對準系統(tǒng)中[11-12]。

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

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

1 非線性快速傳遞對準模型

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

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

(1)

(2)

(3)

1.2 非線性快速傳遞對準模型

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

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

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

(4)

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

(5)

1.2.2非線性快速傳遞對準量測模型

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

(6)

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

(7)

式(6)中：

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

2.1 高階球面-徑向容積準則

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

(8)

其中，xk∈Rn是狀態(tài)向量，zk∈Rm是量測向量，f(·)和h(·)是非線性向量函數(shù)，可分別由非線性快速傳遞對準狀態(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)

一步預測：

(13)

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

(14)

2) 量測更新

計算量測更新容積點：

(15)

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

(16)

計算濾波增益：

(17)

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

(18)

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

性能評估由算法的估計精度和計算量分析兩部分組成。分別以濾波算法的泰勒級數(shù)展開精度和計算復雜度評估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é)將建立計算復雜度模型來評估HCKF算法的計算量，并與GHQF和UKF進行比較分析。

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

HCKF、GHQF和UKF均屬于確定性采樣濾波方法[19]，下面將容積點、積分點和sigma點統(tǒng)稱為采樣點，三種濾波算法的采樣點數(shù)與狀態(tài)向量維數(shù)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計算復雜度建模

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

(31)

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

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

(32)

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

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

(33)

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

(34)

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

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

(35)

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

(36)

由式(31)～(36)，算法的計算復雜度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)表示算法的計算復雜度。

3.2.3算法計算量分析

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

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

3.3 性能評估在大失準角傳遞對準中的應用

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

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

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

4 試驗校驗

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

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

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

基于三種非線性濾波算法的快速傳遞對準搖擺臺試驗安裝偏差角估計誤差曲線(航向安裝偏差角20°工況)如圖2～圖4所示，10次搖擺臺試驗對準精度統(tǒng)計結(jié)果(安裝偏差角估計值與光學瞄準獲得的真值相比)如表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三種濾波算法在傳遞對準中的試驗結(jié)果可知，從對準精度角度分析HCKF和GHQF的估計精度相當，比UKF高17%。從計算耗時角度分析，HCKF單次計算耗時為0.187 ms稍高于UKF的0.024 ms，但比GHQF的23.83 ms小2個數(shù)量級；UKF和HCKF計算耗時小于慣導的更新周期(10 ms)，而GHQF的計算耗時大于慣導的更新周期，不能滿足實時計算需求。HCKF在有效提高對準精度的同時又能保證系統(tǒng)的實時性，綜合性能更優(yōu)，具有更加廣闊的運用前景；傳遞對準精度統(tǒng)計和計算耗時結(jié)果與HCKF算法精度和計算量分析結(jié)果一致，校驗了基于多變量函數(shù)泰勒級數(shù)展開和計算復雜度分析的性能評估方法的有效性。

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

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

5 結(jié) 論

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