蘇炳志,穆榮軍,龍 騰,程 超,崔乃剛
(1. 哈爾濱工業(yè)大學(xué)航天學(xué)院,哈爾濱 150001;2. 北京機(jī)電工程研究所,北京 100074)
傳遞對(duì)準(zhǔn)是指在自然擾動(dòng)或戰(zhàn)術(shù)機(jī)動(dòng)的動(dòng)態(tài)環(huán)境下,利用已處于導(dǎo)航狀態(tài)的高精度主慣導(dǎo)系統(tǒng)對(duì)子慣導(dǎo)系統(tǒng)進(jìn)行初始對(duì)準(zhǔn)的一種方法。1989年美國學(xué)者Kain等首次提出基于“速度+姿態(tài)”匹配的快速傳遞對(duì)準(zhǔn)方法[1],提高了對(duì)準(zhǔn)收斂速度和減小了對(duì)運(yùn)動(dòng)平臺(tái)的機(jī)動(dòng)要求,在快速響應(yīng)的艦載機(jī)和艦載武器系統(tǒng)中得到廣泛的應(yīng)用。艦載機(jī)著陸在艦船甲板上的位置是任意的,而艦船主慣導(dǎo)系統(tǒng)卻安裝在甲板下方的導(dǎo)航室內(nèi);此時(shí),主子慣導(dǎo)之間的失準(zhǔn)角可能很大,需要建立大失準(zhǔn)角傳遞對(duì)準(zhǔn)模型[2-3]。
基于貝葉斯框架的非線性濾波和估計(jì)為大失準(zhǔn)角傳遞對(duì)準(zhǔn)系統(tǒng)提供了最優(yōu)的解決方案,特別是利用不同采樣策略對(duì)非線性高斯系統(tǒng)狀態(tài)的后驗(yàn)均值和方差進(jìn)行近似的高斯濾波。其中,UKF(Unscented Kalman filter)通過使用一組sigma點(diǎn)經(jīng)非線性函數(shù)傳遞后的加權(quán)和來逼近狀態(tài)的概率密度函數(shù),但只有3階精度[4-6];為了提高估計(jì)精度,Arasaratnam等[7]基于高斯厄米特積分(Gaussian-Hermite quadrature,GHQ)提出了GHQF(Gaussian-Hermite quadrature filter),能夠達(dá)到任意階的估計(jì)精度,但由于多維GHQF是由單維GHQ準(zhǔn)則通過張量積直接擴(kuò)展得到的,因此GHQF的積分點(diǎn)隨著狀態(tài)維數(shù)的增加呈指數(shù)增長,即“維數(shù)災(zāi)難”;針對(duì)高維狀態(tài)估計(jì),Arasaratnam等[8]基于3階球面-徑向容積準(zhǔn)則,提出了容積卡爾曼濾波(Cubature Kalman filter, CKF),文獻(xiàn)[9-10]基于Genz積分和矩匹配法推導(dǎo)了任意階的球面積分和徑向積分,從而建立了高階容積卡爾曼濾波。HCKF(High-degree cubature Kalman filter)作為一種新興的濾波算法被運(yùn)用于對(duì)準(zhǔn)系統(tǒng)中[11-12]。
針對(duì)非線性濾波算法的性能評(píng)估,國內(nèi)外學(xué)者的研究重點(diǎn)主要在濾波估計(jì)精度分析[13-16],較少有分析算法計(jì)算量的。運(yùn)用泰勒級(jí)數(shù)展開精度分析法:文獻(xiàn)[13-14]分析了UKF算法的估計(jì)精度,文獻(xiàn)[15]比較分析了UKF和CKF的估計(jì)精度,文獻(xiàn)[16]對(duì)UKF、CKF和GHQF三種濾波算法的精度進(jìn)行了比較分析。文獻(xiàn)[17]運(yùn)用計(jì)算復(fù)雜度分析了傳遞對(duì)準(zhǔn)濾波器的計(jì)算量,但僅適用于標(biāo)準(zhǔn)卡爾曼濾波。
本文以艦機(jī)大失準(zhǔn)角傳遞對(duì)準(zhǔn)為應(yīng)用背景,建立非線性快速傳遞對(duì)準(zhǔn)模型;基于高階球面-徑向容積準(zhǔn)則建立HCKF算法模型;綜合運(yùn)用多變量函數(shù)泰勒級(jí)數(shù)展開和計(jì)算復(fù)雜度分析對(duì)HCKF的估計(jì)精度和計(jì)算量予以分析,并與GHQF和UKF進(jìn)行比較,綜合評(píng)估濾波算法性能;參照GHQF和UKF,分別通過艦機(jī)大失準(zhǔn)角傳遞對(duì)準(zhǔn)搖擺臺(tái)試驗(yàn)中的對(duì)準(zhǔn)精度統(tǒng)計(jì)和計(jì)算耗時(shí)加以驗(yàn)證,從而綜合驗(yàn)證HCKF性能評(píng)估方法的有效性。
以“北-天-東”地理系為導(dǎo)航系,大失準(zhǔn)角下的非線性快速傳遞對(duì)準(zhǔn)誤差模型為[1,18]:
(1)
(2)
(3)
考慮到對(duì)準(zhǔn)時(shí)間短,對(duì)陀螺儀零偏和加速度計(jì)零偏沒有估計(jì)效果,且高精度的器件誤差對(duì)對(duì)準(zhǔn)精度影響小,為了減小計(jì)算量,提高運(yùn)行速度,因此未將其列入傳遞對(duì)準(zhǔn)狀態(tài)向量中;鑒于本文的研究重點(diǎn)是進(jìn)行HCKF算法的性能評(píng)估,而非撓曲變形建模,因此在初始對(duì)準(zhǔn)過程中,認(rèn)為主、子慣導(dǎo)之間的位置是固定不變的。根據(jù)式(1),構(gòu)建9維非線性快速傳遞對(duì)準(zhǔn)模型,采用“速度+姿態(tài)”匹配實(shí)現(xiàn)艦機(jī)之間的快速傳遞對(duì)準(zhǔn)。
1.2.1非線性快速傳遞對(duì)準(zhǔn)狀態(tài)模型
選擇量測失準(zhǔn)角矢量ψm,速度誤差矢量δV和安裝偏差角矢量ψa為傳遞對(duì)準(zhǔn)狀態(tài)向量,即:
(4)
根據(jù)式(1)可得到狀態(tài)模型微分方程:
(5)
1.2.2非線性快速傳遞對(duì)準(zhǔn)量測模型
選擇速度誤差矢量δV和量測失準(zhǔn)角矢量ψm為傳遞對(duì)準(zhǔn)量測向量,即:
(6)
式中:Vm,Vs分別為主慣導(dǎo)和子慣導(dǎo)的速度矢量,g(·)表示從方向余弦矩陣求取歐拉角的函數(shù),矩陣Cz的定義如下:
(7)
式(6)中:
考慮如下非線性離散系統(tǒng)狀態(tài)空間模型:
(8)
其中,xk∈Rn是狀態(tài)向量,zk∈Rm是量測向量,f(·)和h(·)是非線性向量函數(shù),可分別由非線性快速傳遞對(duì)準(zhǔn)狀態(tài)模型和量測模型求得,wk-1和vk分別為過程和量測噪聲,均方誤差矩陣分別為Qk-1和Rk。
(9)
(10)
根據(jù)式(8)和(9)并結(jié)合非線性高斯濾波遞推公式[8]可得到高階(5階)容積卡爾曼濾波的具體計(jì)算步驟:
1) 時(shí)間更新
計(jì)算k-1時(shí)刻的容積點(diǎn)
(11)
其中,n是狀態(tài)向量維數(shù),向量為:
χi=
(12)
一步預(yù)測:
(13)
式中權(quán)值wi為:
(14)
2) 量測更新
計(jì)算量測更新容積點(diǎn):
(15)
經(jīng)量測非線性變換的容積點(diǎn):
(16)
計(jì)算濾波增益:
(17)
狀態(tài)估計(jì)值及協(xié)方差陣計(jì)算:
(18)
性能評(píng)估由算法的估計(jì)精度和計(jì)算量分析兩部分組成。分別以濾波算法的泰勒級(jí)數(shù)展開精度和計(jì)算復(fù)雜度評(píng)估HCKF估計(jì)精度和計(jì)算量,并與GHQF和UKF進(jìn)行比較。
3.1.1多變量函數(shù)泰勒級(jí)數(shù)展開
(19)
3.1.2高階容積卡爾曼濾波估計(jì)精度
(20)
其中,
δxi=
(21)
根據(jù)式(19),并考慮到2n2+1個(gè)HCKF算法的容積點(diǎn)是對(duì)稱的,奇數(shù)階矩相互抵消,則映射后f(x)的均值為:
(22)
其中,二階矩計(jì)算過程為:
(23)
若考慮其他偶數(shù)階泰勒級(jí)數(shù)展開項(xiàng),i=1,2,…,2n時(shí)
(24)
同理,i=2n+1,2n+2,…,2n2時(shí)
(25)
根據(jù)式(24)和(25)可得:
(26)
由式(23)和(26)可得:
(27)
3.1.3GHQF和UKF估計(jì)精度
(28)
(29)
3.1.4估計(jì)精度比較
將式(27)~(29)分別與多變量函數(shù)泰勒級(jí)數(shù)展開式(19)比較可知,高階(5階)容積卡爾曼濾波能精確傳播至5階項(xiàng),從6階項(xiàng)開始出現(xiàn)截?cái)嗾`差,即有5階泰勒級(jí)數(shù)展開精度;GHQF的估計(jì)精度由單維高斯厄米特積分點(diǎn)數(shù)q決定,qn個(gè)積分點(diǎn)的多維高斯厄米特積分濾波能夠精確傳播至2q-1階項(xiàng),從2q階項(xiàng)開始出現(xiàn)截?cái)嗾`差。UKF能精確傳播至3階項(xiàng),從4階項(xiàng)開始出現(xiàn)截?cái)嗾`差,即有3階泰勒級(jí)數(shù)展開精度。
對(duì)于一種濾波算法不僅需要考慮它的估計(jì)精度,而且還必須考慮它的計(jì)算量。本節(jié)將建立計(jì)算復(fù)雜度模型來評(píng)估HCKF算法的計(jì)算量,并與GHQF和UKF進(jìn)行比較分析。
3.2.1三種濾波采樣點(diǎn)數(shù)
HCKF、GHQF和UKF均屬于確定性采樣濾波方法[19],下面將容積點(diǎn)、積分點(diǎn)和sigma點(diǎn)統(tǒng)稱為采樣點(diǎn),三種濾波算法的采樣點(diǎn)數(shù)與狀態(tài)向量維數(shù)n的關(guān)系如式(30)和表1所示:
(30)
其中,為了更好地比較分析HCKF算法的性能,GHQF的估計(jì)精度也取5階,即q=3。
表1 HCKF、GHQF和UKF采樣點(diǎn)數(shù)Table 1 Number of sampling points for HCKF,GHQF and UKF
3.2.2計(jì)算復(fù)雜度建模
求取k-1時(shí)刻容積點(diǎn)式(11)的計(jì)算復(fù)雜度
(31)
式中:Tm1(n)表示乘法個(gè)數(shù),Ta1(n)表示加法個(gè)數(shù),Cholesky分解(平方根法)的計(jì)算復(fù)雜度n3/3按加法計(jì)算。
一步預(yù)測式(13)的計(jì)算復(fù)雜度
(32)
式中:Mx和Ax分別表示非線性傳遞函數(shù)f(x)的乘法和加法運(yùn)算個(gè)數(shù),Nm為采樣點(diǎn)數(shù)。
求取量測更新容積點(diǎn)式(15)的計(jì)算復(fù)雜度
(33)
求取經(jīng)量測非線性變換容積點(diǎn)式(16)的計(jì)算復(fù)雜度
(34)
式中:Mz和Az分別表示非線性量測函數(shù)h(x)的乘法和加法運(yùn)算個(gè)數(shù)。
濾波增益計(jì)算式(17)的計(jì)算復(fù)雜度
(35)
狀態(tài)估計(jì)及方差矩陣更新式(18)的計(jì)算復(fù)雜度
(36)
由式(31)~(36),算法的計(jì)算復(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)表示乘法個(gè)數(shù),Ta(n)表示加法個(gè)數(shù),T(n)表示算法的計(jì)算復(fù)雜度。
3.2.3算法計(jì)算量分析
量測向量維數(shù)m一般小于狀態(tài)向量維數(shù)n,因此HCKF的計(jì)算復(fù)雜度與狀態(tài)維數(shù)n的四次方成正相關(guān),即T(n)=O(n4);而GHQF的計(jì)算復(fù)雜度隨著狀態(tài)維數(shù)n的增加呈指數(shù)增長趨勢(shì),即T(n)=O(n2×3n)[20];UKF的計(jì)算復(fù)雜度與狀態(tài)維數(shù)n的三次方成正相關(guān),即T(n)=O(n3)[20]。
由于計(jì)算復(fù)雜度越大,算法的計(jì)算量越大,因此根據(jù)上述理論分析可知,在相同精度等級(jí)條件下HCKF的計(jì)算量比GHQF小。
將基于多變量函數(shù)泰勒級(jí)數(shù)展開和計(jì)算復(fù)雜度分析的性能評(píng)估方法運(yùn)用到大失準(zhǔn)角傳遞對(duì)準(zhǔn)中。
根據(jù)第3.1.4節(jié)估計(jì)精度比較可知,HCKF與GHQF都具有5階泰勒級(jí)數(shù)展開精度,高于UKF的3階泰勒級(jí)數(shù)展開精度,在艦機(jī)大失準(zhǔn)角傳遞對(duì)準(zhǔn)等強(qiáng)非線性系統(tǒng)中更加能突顯HCKF的精度優(yōu)勢(shì)。根據(jù)第3.2.3節(jié)算法計(jì)算量分析可知,對(duì)于大失準(zhǔn)角傳遞對(duì)準(zhǔn)模型(n=9),此時(shí)HCKF的計(jì)算量比UKF的高1個(gè)數(shù)量級(jí),但比GHQF的小2個(gè)數(shù)量級(jí)。
綜合上述分析,HCKF能夠有效提高艦機(jī)大失準(zhǔn)角傳遞對(duì)準(zhǔn)精度,同時(shí)又不會(huì)致使計(jì)算量急劇增大,綜合性能更優(yōu),更加有望運(yùn)用于工程實(shí)際中。
為了校驗(yàn)高階(5階)容積卡爾曼濾波性能評(píng)估的理論分析結(jié)果,開展了快速傳遞對(duì)準(zhǔn)搖擺臺(tái)試驗(yàn)。為了盡量排除干擾,在相對(duì)比較理想的試驗(yàn)條件下進(jìn)行試驗(yàn)驗(yàn)證,能更加精準(zhǔn)的評(píng)估HCKF算法性能,因此試驗(yàn)主子慣導(dǎo)之間是剛性連接的。利用六自由度搖擺臺(tái)模擬中等海況[3]:載體的俯仰角?,橫滾角γ,偏航角ψ的幅值和周期分別為5°和8 s,橫蕩、縱蕩和垂蕩引起的線速度幅值和周期分別為0.1 m和10 s。搖擺臺(tái)試驗(yàn)系統(tǒng)組成及主子慣導(dǎo)器件指標(biāo)如圖1所示。
圖1 快速傳遞對(duì)準(zhǔn)搖擺臺(tái)試驗(yàn)系統(tǒng)結(jié)構(gòu)Fig.1 The rapid transfer alignment swing bench test system
為了全面考核與驗(yàn)證算法對(duì)安裝偏差角估計(jì)的正確性,通過三軸電控角位移臺(tái)改變主子慣導(dǎo)之間的安裝偏差角,水平安裝偏差角ψax=3°,ψaz=3°,航向安裝偏差角可取ψay=±10°,±20°, ±30°,±40°,±50°,共10個(gè)試驗(yàn)工況。
基于三種非線性濾波算法的快速傳遞對(duì)準(zhǔn)搖擺臺(tái)試驗(yàn)安裝偏差角估計(jì)誤差曲線(航向安裝偏差角20°工況)如圖2~圖4所示,10次搖擺臺(tái)試驗(yàn)對(duì)準(zhǔn)精度統(tǒng)計(jì)結(jié)果(安裝偏差角估計(jì)值與光學(xué)瞄準(zhǔn)獲得的真值相比)如表2所示,三種濾波算法單次計(jì)算耗時(shí)(硬件配置:Core i7-4790 CPU,軟件配置:Visual Studio 2005)如表3所示。
圖3 Y軸安裝偏差角估計(jì)誤差Fig.3 Estimation error of installation deviation angle of Y axis
圖4 Z軸安裝偏差角估計(jì)誤差Fig.4 Estimation error of installation deviation angle of Z axis
由圖2~4和表2~3三種濾波算法在傳遞對(duì)準(zhǔn)中的試驗(yàn)結(jié)果可知,從對(duì)準(zhǔn)精度角度分析HCKF和GHQF的估計(jì)精度相當(dāng),比UKF高17%。從計(jì)算耗時(shí)角度分析,HCKF單次計(jì)算耗時(shí)為0.187 ms稍高于UKF的0.024 ms,但比GHQF的23.83 ms小2個(gè)數(shù)量級(jí);UKF和HCKF計(jì)算耗時(shí)小于慣導(dǎo)的更新周期(10 ms),而GHQF的計(jì)算耗時(shí)大于慣導(dǎo)的更新周期,不能滿足實(shí)時(shí)計(jì)算需求。HCKF在有效提高對(duì)準(zhǔn)精度的同時(shí)又能保證系統(tǒng)的實(shí)時(shí)性,綜合性能更優(yōu),具有更加廣闊的運(yùn)用前景;傳遞對(duì)準(zhǔn)精度統(tǒng)計(jì)和計(jì)算耗時(shí)結(jié)果與HCKF算法精度和計(jì)算量分析結(jié)果一致,校驗(yàn)了基于多變量函數(shù)泰勒級(jí)數(shù)展開和計(jì)算復(fù)雜度分析的性能評(píng)估方法的有效性。
表2 10次傳遞對(duì)準(zhǔn)試驗(yàn)RMSE統(tǒng)計(jì)結(jié)果Table 2 Statistics results of transfer alignmentRMSE in 10 tests
表3 三種濾波算法計(jì)算耗時(shí)比較Table 3 Comparison of filtering computation times
本文以艦機(jī)大失準(zhǔn)角傳遞對(duì)準(zhǔn)為應(yīng)用背景,建立了非線性快速傳遞對(duì)準(zhǔn)模型;基于高階球面-徑向容積準(zhǔn)則建立了HCKF算法模型,利用多變量泰勒級(jí)數(shù)展開和計(jì)算復(fù)雜度分析衡量HCKF的估計(jì)精度和計(jì)算量,從而綜合評(píng)估HCKF的性能,并與GHQF和UKF進(jìn)行比較。理論分析表明:HCKF和GHQF均具有5階泰勒級(jí)數(shù)展開精度,高于UKF的3階精度;HCKF的計(jì)算復(fù)雜度O(n4)比UKF的O(n3)高一個(gè)數(shù)量級(jí)(n=9),但比GHQF的O(n2×3n)小2個(gè)數(shù)量級(jí)。艦機(jī)大失準(zhǔn)角傳遞對(duì)準(zhǔn)搖擺臺(tái)試驗(yàn)校驗(yàn)表明:HCKF的估計(jì)精度與GHQF相當(dāng),比UKF高17%,計(jì)算量稍高于UKF,但比GHQF小2個(gè)數(shù)量級(jí),滿足實(shí)時(shí)計(jì)算需求;HCKF能夠兼顧估計(jì)精度與計(jì)算量,綜合性能更優(yōu),運(yùn)用前景更加廣闊;試驗(yàn)結(jié)果與HCKF的性能評(píng)估理論分析結(jié)果一致,校驗(yàn)了性能評(píng)估算法的有效性。