王律化，石志勇，宋金龍，王海亮

(陸軍工程大學(xué)石家莊校區(qū)，河北 石家莊 050003)

慣性導(dǎo)航初始對準(zhǔn)是確定初始導(dǎo)航參數(shù)的過程，分為粗對準(zhǔn)和精對準(zhǔn)兩個(gè)階段。精對準(zhǔn)是對于粗對準(zhǔn)建立的姿態(tài)矩陣進(jìn)行進(jìn)一步修正的過程，精對準(zhǔn)的研究主要集中在誤差方程和濾波算法方面。靜基座條件下，系統(tǒng)誤差模型主要有兩種：一種是表示計(jì)算坐標(biāo)系和導(dǎo)航坐標(biāo)系之間失準(zhǔn)角ψ誤差模型；另一種是表示真坐標(biāo)系和導(dǎo)航坐標(biāo)系之間誤差角Φ的誤差模型，利用誤差角為小角度的特點(diǎn)，可將誤差方程進(jìn)行線性化，滿足線性卡爾曼濾波的要求，完成初始對準(zhǔn)。隨著對載體機(jī)動性要求的提高，初始對準(zhǔn)由原來的靜基座初始對準(zhǔn)逐漸向行進(jìn)間初始對準(zhǔn)轉(zhuǎn)化，小角度的誤差模型和線性的濾波理論已經(jīng)不能再滿足初始對準(zhǔn)的要求。

針對載體行進(jìn)間初始對準(zhǔn)精度問題，在里程計(jì)輔助慣性系行進(jìn)間精對準(zhǔn)的情況下，推導(dǎo)了精對準(zhǔn)誤差模型。通過降維處理和狀態(tài)方程參量的自相關(guān)函數(shù)的正交化相結(jié)合，在降低濾波計(jì)算量的基礎(chǔ)上，保證了濾波器在非白噪聲情況下的濾波器的穩(wěn)定性。由于在濾波過程中需要對于參量的后驗(yàn)概率密度進(jìn)行數(shù)值解算，為提高解算精度，運(yùn)用5階球面-徑向準(zhǔn)則進(jìn)行計(jì)算。仿真實(shí)驗(yàn)表明，在方位角為大失準(zhǔn)角的條件下，該算法可以有效地保證較高的濾波精度，并且在噪聲未知的情況下，濾波器保證很好的魯棒性。

1 行進(jìn)間捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)誤差模型

捷聯(lián)慣導(dǎo)系統(tǒng)完成行進(jìn)間粗對準(zhǔn)后，根據(jù)姿態(tài)矩陣的解算，其水平失準(zhǔn)角是小角度，而方位失準(zhǔn)角為大角度。因此，進(jìn)行行進(jìn)間初始對準(zhǔn)精對準(zhǔn)的過程中，其誤差模型應(yīng)當(dāng)使用大方位角誤差模型。根據(jù)文獻(xiàn)[1]和[10]，行進(jìn)間捷聯(lián)慣導(dǎo)大方位角誤差模型表示為

(1)

捷聯(lián)慣導(dǎo)實(shí)測位置和里程計(jì)的實(shí)測位置的差值作為量測值

(2)

綜上所述，行進(jìn)間大方位失準(zhǔn)角誤差方程為

(3)

式中:f(x)的表達(dá)式為式(1);噪聲向量w和v是零均值，方差為Q和R的高斯白噪聲;量測矩陣表示為：H=[03×303×3I3×3-I3×303×303×4]T.

因此，式(3)可以改寫為：

(4)

式中：F(α)為狀態(tài)矩陣；H為量測矩陣；g(α)和h(α)為非線性函數(shù)。

2 貝葉斯濾波過程及其改進(jìn)方法

2.1 貝葉斯濾波過程

將式(4)進(jìn)行離散化處理，可表示成：

(5)

貝葉斯估計(jì)的預(yù)測和更新過程中，都涉及到如下形式的多維微積分：

(6)

2.2 強(qiáng)跟蹤貝葉斯濾波過程

上述貝葉斯估計(jì)，要求系統(tǒng)的噪聲是高斯白噪聲，但是在實(shí)際的工作狀態(tài)下，系統(tǒng)噪聲的統(tǒng)計(jì)學(xué)特性式未知的，這種條件下，單純的使用貝葉斯濾波過程，會使得系統(tǒng)的濾波效果變差甚至造成濾波器的發(fā)散。為了提高濾波器的魯棒性，使其在噪聲條件未知的情況下，依舊有著很好的濾波性能和估計(jì)精度，并且在濾波過程中，在噪聲發(fā)生突變的條件下，依舊使得濾波器的性能穩(wěn)定。為滿足上述要求，將強(qiáng)跟蹤濾波(STF)和高階容積濾波將結(jié)合，在保證濾波精度的情況下，使得濾波器的魯棒性得到很好的提升。

將貝葉斯濾波過程寫成如下形式：

Pk|k-1=Fk,k-1Pk-1Fk,k-1+Qk，

(7)

Pk|k=(I-KkHk)Pk|k-1，

(8)

(9)

式中，F(xiàn)k,k-1表示從k-1時(shí)刻到k時(shí)刻的狀態(tài)一步轉(zhuǎn)移矩陣。

將次優(yōu)漸消因子ηk代入到式(7)中得：

Pk|k-1=ηkFk,k-1Pk-1Fk,k-1+Qk，

(10)

(11)

(12)

(13)

(14)

式中:tr[·]表示矩陣跡的計(jì)算；ρ的取值為0<ρ≤1，一般情況下取為ρ=0.95，在系統(tǒng)的噪聲未確定的情況下，運(yùn)用式(14)解算系統(tǒng)噪聲， 使得系統(tǒng)噪聲類似于高斯白噪聲，從而便于進(jìn)行解算。

3 5階容積濾波解算方法

前文中關(guān)于強(qiáng)跟蹤高階容積濾波中參數(shù)的數(shù)值解算涉及到高維積分，因此，對于式(6)變形得：

(15)

式中：ri和wr,i是徑向積分的采樣點(diǎn)和采樣點(diǎn)對應(yīng)的權(quán)值；sj和ws,j是球面積分的采樣點(diǎn)和采樣點(diǎn)對應(yīng)的權(quán)值；I(g)的總采樣地點(diǎn)為NrNs個(gè)，當(dāng)ri中有一個(gè)采樣點(diǎn)為0時(shí)，I(g)的總采樣點(diǎn)數(shù)為(Nr-1)(Ns+1)個(gè)。

3.1 5階球面積分方法

(16)

式(16)為2m+1階球面積分?jǐn)?shù)值解算方法，式中IUn表示被解算的函數(shù)。式中的權(quán)值函數(shù)和采樣點(diǎn)的函數(shù)值之和分別為

(17)

(18)

對于高維積分的數(shù)值解算，有如下公式：

(19)

當(dāng)所要近似的積分階數(shù)是5階的時(shí)候，2m+1=5，則m=2，將結(jié)果帶入到式(18)中得：

ws2=wp(2,0,…,0)=

(20)

(21)

ωs1所對應(yīng)的采樣點(diǎn)為

ws2所對應(yīng)的采樣點(diǎn)為：ej和-ej，其中j=1,2,…,n。

將式(20)和(21)以及采樣點(diǎn)代入到式(16)中,則有

(22)

3.2 徑向積分法則

對于5階及5階以下的高維徑向積分，可以使用高斯-拉格朗日積分方法，但是，這個(gè)方法不能運(yùn)用于5維以上的數(shù)值解算，為提高徑向高維積分解算方法的適用性，采用時(shí)矩匹配法解算徑向積分。時(shí)矩匹配法具體表示為：

(23)

式中:gr(r)=rl，rl∈[0,r2,…,r2(Nr-1)]，l是一個(gè)非負(fù)偶數(shù);并且，當(dāng)gr(r)=rl時(shí)，

因此，當(dāng)運(yùn)用時(shí)矩匹配法解算5階徑向高維積分時(shí)，得到如下方程：

(24)

式中,Γ(z+1)=zΓ(z)，由于式(24)式中有4個(gè)未知數(shù)，只有3個(gè)方程，同時(shí)為了保證采用時(shí)矩匹配法數(shù)值解算徑向積分的采樣點(diǎn)數(shù)最少，可以令r1=0，于是求得式(24)的解為

(25)

將式(22)、(25)代入式(15)中，運(yùn)用5階容積規(guī)則(Nr=2,Ns=2n2)解算得：

(26)

(27)

(28)

式(27)中，

Φk-1(αk-1)=Fk-1(αk-1)·

(29)

式(28)中，

(30)

式(29)和(30)中的Sk-1為對應(yīng)變量的Cholesky分解,即Pαk-1=Sαk-1(Sαk-1)T.

4 仿真實(shí)驗(yàn)

載體在完成行進(jìn)間精對準(zhǔn)的過程中，為了驗(yàn)證強(qiáng)跟蹤5階CKF算法對于載體狀體的估計(jì)精度和估計(jì)時(shí)間，以及在噪聲未知條件下的魯棒性，通過和3階CKF算法在系統(tǒng)誤差模型確定的條件下，3個(gè)方向失準(zhǔn)角的對比以及在噪聲條件未知的情況下，以航向失準(zhǔn)角為例，對比強(qiáng)跟蹤5階CKF和3階CKF的估計(jì)效果，說明強(qiáng)跟蹤5階CKF算法的估計(jì)精度和魯棒性。

載體總的精對準(zhǔn)仿真時(shí)間為600 s，假設(shè)載體的初始位置為45.235 1°N/85.238 4°E，高度0 m，陀螺為激光陀螺，其常值漂移為0.015(°)/h，隨機(jī)漂移為0.001(°)/h，加速度計(jì)常值零偏為450 μg，隨機(jī)漂移為10 μg，里程計(jì)的刻度系數(shù)誤差為2‰，載體在完成粗對準(zhǔn)后的失準(zhǔn)角為(3°，3°，10°) 仿真結(jié)果如圖1～3所示。

從圖1～3可以看出，當(dāng)失準(zhǔn)角是小角度的時(shí)候，3階CKF濾波和強(qiáng)跟蹤5階CKF濾波的濾波效果相近。圖3中表明，當(dāng)失準(zhǔn)角為大角度的時(shí)候，強(qiáng)跟蹤5階CKF濾波相較于3階CKF濾波方式，其對于失準(zhǔn)角的估計(jì)精度有所提高，就估計(jì)時(shí)間而言，3階CKF收斂的時(shí)間為345 s，而強(qiáng)跟蹤5階CKF中徑向積分由于采用時(shí)矩匹配法解算，相較于采用拉格朗日方程解算的3階CKF算法，其結(jié)構(gòu)復(fù)雜，因此，強(qiáng)跟蹤5階CKF濾波在461 s以后開始收斂，有著明顯的滯后。

當(dāng)系統(tǒng)噪聲信號的統(tǒng)計(jì)學(xué)特性變?yōu)镋[wk]=0.2vb,E[vk]=0.3vb，即系統(tǒng)噪聲的統(tǒng)計(jì)學(xué)特性和速度相關(guān)。系統(tǒng)的濾波結(jié)果如圖4所示。

從圖4中看出，當(dāng)系統(tǒng)的噪聲不能看成高斯白噪聲的情況下，由于不符合3階CKF濾波的噪聲假設(shè)，使得對于航向失準(zhǔn)角的估計(jì)不能隨著時(shí)間的推移而有所收斂。但是強(qiáng)跟蹤5階CKF濾波，由于引進(jìn)了將次優(yōu)漸消因子ηk，從而使得有色噪聲白化，使得最終的濾波效果可以收斂，從而實(shí)現(xiàn)最終的對準(zhǔn)。

5 結(jié)論

針對載體行進(jìn)間初始對準(zhǔn)精度問題，在里程計(jì)輔助慣性系行進(jìn)間精對準(zhǔn)的情況下，推導(dǎo)了精對準(zhǔn)誤差模型。通過對于強(qiáng)跟蹤5階CKF濾波和3階CKF的結(jié)果對比，得出以下結(jié)論：

載體采用強(qiáng)跟蹤5階CKF濾波方式估計(jì)載體行進(jìn)間精對準(zhǔn)的參數(shù)，并且在濾波過程中，針對狀態(tài)方程維數(shù)高的問題，進(jìn)行了降維處理。對比3階CKF算法，強(qiáng)跟蹤5階CKF濾波在處理時(shí)間上延長了33.4%，但是計(jì)算精度上提高了25.2%；并且在非白噪聲的條件下，濾波器保持良好的魯棒性。