強(qiáng)跟蹤五階CKF算法在初始對準(zhǔn)中的應(yīng)用

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

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

0 引言

慣性導(dǎo)航初始對準(zhǔn)是確定初始導(dǎo)航參數(shù)的過程，分為粗對準(zhǔn)和精對準(zhǔn)兩個階段。精對準(zhǔn)通過分析各個誤差源對于對準(zhǔn)結(jié)果的影響，進(jìn)一步修正姿態(tài)矩陣。現(xiàn)階段，精對準(zhǔn)的研究主要集中在大失準(zhǔn)誤差模型和非線性誤差模型的濾波算法方面。靜基座條件下，系統(tǒng)誤差模型主要是Φ和ψ的小角度誤差模型，運(yùn)用誤差角為小角度的特點(diǎn)，可將誤差模型進(jìn)行線性化，滿足線性卡爾曼濾波的要求，完成初始對準(zhǔn)。隨著載體機(jī)動性要求的提高，初始對準(zhǔn)由靜基座初始對準(zhǔn)逐漸向行進(jìn)間初始對準(zhǔn)轉(zhuǎn)化，小失準(zhǔn)角的誤差模型和線性的濾波方法已不能滿足初始對準(zhǔn)的要求。

本文針對載體行進(jìn)間初始對準(zhǔn)精度問題，在由里程計輔助慣性系統(tǒng)行進(jìn)間精對準(zhǔn)的情況下，推導(dǎo)了精對準(zhǔn)誤差模型，并建立了導(dǎo)航系統(tǒng)精對準(zhǔn)的狀態(tài)方程和量測方程。系統(tǒng)方程為非線性方程，系統(tǒng)狀態(tài)的估計采用貝葉斯濾波方式，為提高濾波器在非白噪聲條件下的魯棒性，在計算貝葉斯濾波過程中的增益矩陣時引入自適應(yīng)漸消因子。濾波過程中需要對于參量的后驗概率密度進(jìn)行數(shù)值解算，為提高解算精度，運(yùn)用五階球面-徑向準(zhǔn)則進(jìn)行計算。仿真和實驗結(jié)果表明，在方位角為大失準(zhǔn)角的條件下，該算法可有效地保證較高的濾波精度，并且在噪聲未知的情況下，濾波器保持很好的魯棒性。

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

(1)

其中

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

(10)

取狀態(tài)向量x=[φ(δvn) (δPn) (δPnD)bεbδKD]T，其中[]T為狀態(tài)向量的轉(zhuǎn)置。

噪聲向量為

(11)

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

(12)

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

(13)

式(13)中f(x)的表達(dá)式具體見式(1)，其中狀態(tài)噪聲向量w和量測噪聲v是零均值，方差為Q和R的高斯白噪聲。量測矩陣為

(14)

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

2.1 貝葉斯濾波過程

處理式(13)的濾波方法一般為貝葉斯濾波。將式(13)進(jìn)行離散化處理,則有

(15)

式中wk-1和vk是獨(dú)立的狀態(tài)高斯白噪聲和量測高斯白噪聲，其方差矩陣分別為Qk-1和Rk。

式(15)的貝葉斯濾波過程分為以下兩步，即

1) 預(yù)測過程。

(16)

(17)

2) 更新過程。

(18)

(19)

(20)

(21)

(22)

(23)

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

(24)

對于式(24)的多維積分計算其解析解幾乎不可能，一般是求解式(24)的近似數(shù)值解。根據(jù)近似求解的方法不同，將式(16)～(23)的濾波過程稱為不同的濾波方法。根據(jù)容積近似計算方法[6]，系統(tǒng)噪聲是高斯白噪聲的情況下，可以對式(24)進(jìn)行如下轉(zhuǎn)換,即

(25)

式中:P=SST，S是P的平方根分解;NP為總采樣點(diǎn)數(shù)；γi和Wi分別為采樣點(diǎn)和采樣點(diǎn)權(quán)重。

由式(25)的解算方法應(yīng)用到式(16)～(23)得:

預(yù)測過程：

(26)

(27)

式中:Pk-1|k-1=SST;ξi=Sγi+k-1|k-1。

更新過程：

(28)

(29)

(30)

式(26)、(27)、(18)～(20)、(28)～(30)構(gòu)成了貝葉斯濾波器數(shù)值解算的全部過程，式中Wi和γi的計算方法，則通過球面-徑向準(zhǔn)則進(jìn)行計算。

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

上述貝葉斯估計，要求系統(tǒng)的噪聲是高斯白噪聲，但是在實際的工作狀態(tài)下，系統(tǒng)噪聲的統(tǒng)計學(xué)特性是未知的，此條件下，使用貝葉斯濾波，使得系統(tǒng)的濾波效果變差甚至造成濾波器的發(fā)散。為提高濾波器的魯棒性，同時，在噪聲條件未知的情況下，保持良好的濾波精度。將強(qiáng)跟蹤濾波(STF)和高階容積濾波結(jié)合，在保證濾波精度的情況下，使濾波器的魯棒性得到很好的提升。

將貝葉斯濾波過程的式(17)，(19),(20)進(jìn)行等價變形得

(31)

(32)

(33)

式(31)中Fk,k-1是式(15)中f(xk-1)的離散化矩陣，表示從k-1時刻到k時刻的狀態(tài)一步轉(zhuǎn)移矩陣。

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

(34)

根據(jù)文獻(xiàn)[9]中正交性原理的要求，ηk≥1，并且輸出的殘差序列為δk=zz-k|k-1，ηk的計算方法為

(35)

(36)

(37)

(38)

(39)

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

3 五階容積濾波解算方法

3.1 高維積分的數(shù)值計算準(zhǔn)則

根據(jù)文獻(xiàn)[11]，高維積分的數(shù)值計算為

(40)

式中：x=[x1x2…xn]T∈Rn；wg(x)g(x)的權(quán)重函數(shù)，g(x)=xα11xα22…xαnn，α1,α2,…,αn是非負(fù)整數(shù)，且α1+α2+…+αn≤d，d是所能達(dá)到的精度的階數(shù)。

根據(jù)容積準(zhǔn)則，形如式(24)的積分寫成下列形式：

(41)

(42)

式(42)中主要涉及了兩種形式的積分:

1) 徑向積分，∫∞0gr(r)rn-1exp(-r2)dr積分的權(quán)重公式為：wg(r)=rn-1exp(-r2)。

2) 球面積分∫Ung(s)dσ(s)，對應(yīng)的權(quán)重函數(shù)為：wg(s)=1。將徑向積分和球面積分運(yùn)用式(40)進(jìn)行變形，式(42)可變?yōu)?/p>

(43)

式中: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)為Nr、Ns，當(dāng)ri中有一個采樣點(diǎn)為0時，I(g)的總采樣點(diǎn)數(shù)為(Nr-1)(Ns+1)。

3.2 五階球面積分方法

根據(jù)文獻(xiàn)[8]，形如IUn(gs)∫Ungs(s)dσ(s)的積分可以按照下式進(jìn)行近似計算，即

(44)

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

(45)

(46)

根據(jù)文獻(xiàn)[8],有如下公式：

(47)

式中:|k|=k1+k2+…+kn;Γ(z)是伽馬函數(shù)，定義Γ(z) = ∫∞0exp(-λ)λz-1dλ。

當(dāng)所要近似的積分階數(shù)是五階時，2m+1=5，則m=2。將結(jié)果代入式(45)中得

(48)

(49)

和

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

將式(48)、(49)及采樣點(diǎn)代入式(41)可得

(50)

3.3 徑向積分法則

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

(51)

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

(52)

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

(53)

(54)

將式(42)、(51)、(53)、(54)代入式(24)中，運(yùn)用五階容積規(guī)則(Nr=2,Ns=2n2)解算得

(55)

4 仿真和實驗

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

4.1 仿真實驗

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

圖1 東向水平失準(zhǔn)角

圖2 北向水平失準(zhǔn)角

圖3 航向失準(zhǔn)角

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

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

圖4 非白噪聲下的濾波

4.2 實車實驗

實車實驗采用裝用GNSS/SINS/BD的組合導(dǎo)航系統(tǒng)的無人車(UGV)進(jìn)行測試，采用高精度差分GPS所測量的路線作為路徑真值，GPS的輸出為1 Hz，定位精度為1 m。里程計的標(biāo)度因數(shù)KD=0.035 45，定位精度度為0.05%(CEP)。實驗過程中，無人車經(jīng)過勻速直線、加速、左轉(zhuǎn)、右轉(zhuǎn)及減速等運(yùn)動過程。整個運(yùn)動時長是900 s。圖5為實驗用無人車。圖6為實驗路徑。

圖5 實驗用無人車

圖6 實驗路徑

以東向定位誤差為例，對比實驗過程中采用強(qiáng)跟蹤五階CKF算法和三階CKF算法的定位誤差。結(jié)果如圖7所示。

圖7 定位誤差對比圖

由圖7可知，當(dāng)捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航系統(tǒng)采用強(qiáng)跟蹤五階CKF濾波算法時，對比捷聯(lián)慣導(dǎo)/衛(wèi)星組合導(dǎo)航系統(tǒng)的輸出結(jié)果的位置誤差為20 m,當(dāng)采用三階CKF濾波算法輸出的位置和捷聯(lián)慣導(dǎo)/衛(wèi)星組合導(dǎo)航系統(tǒng)輸出位置做差，其結(jié)果為29 m，就東向定位精度而言，采用強(qiáng)跟蹤五階CKF濾波算法，其定位精度提高了31.1%。

5 結(jié)束語

通過對強(qiáng)跟蹤五階CKF濾波和三階CKF的結(jié)果對比可知，載體采用強(qiáng)跟蹤五階CKF濾波方式估計載體行進(jìn)間精對準(zhǔn)的參數(shù)，與三階CKF濾波方式相比，在時間上有所延長，其原因主要是采用五階以上的計算方法，在計算點(diǎn)數(shù)增加，使得計算時間有所延長，但濾波精度和濾波器在非白噪聲下的魯棒性有著明顯的提高，極大的增強(qiáng)了濾波器在非白噪聲條件下的適用性。