強跟蹤五階CKF算法在初始對準中的應用

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

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

0 引言

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

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

1 行進間捷聯慣導系統初始對準誤差模型

(1)

其中

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

(10)

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

噪聲向量為

(11)

捷聯慣導實測位置和里程計的實測位置的差值作為量測值

(12)

綜上所述，行進間大方位失準角誤差方程為

(13)

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

(14)

2 貝葉斯濾波過程及其改進方法

2.1 貝葉斯濾波過程

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

(15)

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

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

1) 預測過程。

(16)

(17)

2) 更新過程。

(18)

(19)

(20)

(21)

(22)

(23)

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

(24)

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

(25)

式中:P=SST，S是P的平方根分解;NP為總采樣點數；γi和Wi分別為采樣點和采樣點權重。

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

預測過程：

(26)

(27)

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

更新過程：

(28)

(29)

(30)

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

2.2 強跟蹤貝葉斯濾波過程

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

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

(31)

(32)

(33)

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

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

(34)

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

(35)

(36)

(37)

(38)

(39)

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

3 五階容積濾波解算方法

3.1 高維積分的數值計算準則

根據文獻[11]，高維積分的數值計算為

(40)

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

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

(41)

(42)

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

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

2) 球面積分∫Ung(s)dσ(s)，對應的權重函數為：wg(s)=1。將徑向積分和球面積分運用式(40)進行變形，式(42)可變為

(43)

式中:ri和wr,i分別為徑向積分的采樣點和采樣點對應的權值；sj和ws,j分別為球面積分的采樣點和采樣點對應的權值。I(g)的總采樣點為Nr、Ns，當ri中有一個采樣點為0時，I(g)的總采樣點數為(Nr-1)(Ns+1)。

3.2 五階球面積分方法

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

(44)

式(44)為2m+1階球面積分數值解算方法，式中IUn為被解算的函數。式中的權值函數和采樣點的函數值之和分別為

(45)

(46)

根據文獻[8],有如下公式：

(47)

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

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

(48)

(49)

和

ws2所對應的采樣點為ej和-ej，其中j=1,2,…,n。

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

(50)

3.3 徑向積分法則

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

(51)

因此，當運用時矩匹配法解算五階徑向高維積分時，得到如下方程：

(52)

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

(53)

(54)

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

(55)

4 仿真和實驗

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

4.1 仿真實驗

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

圖1 東向水平失準角

圖2 北向水平失準角

圖3 航向失準角

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

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

圖4 非白噪聲下的濾波

4.2 實車實驗

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

圖5 實驗用無人車

圖6 實驗路徑

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

圖7 定位誤差對比圖

由圖7可知，當捷聯慣導/里程計組合導航系統采用強跟蹤五階CKF濾波算法時，對比捷聯慣導/衛星組合導航系統的輸出結果的位置誤差為20 m,當采用三階CKF濾波算法輸出的位置和捷聯慣導/衛星組合導航系統輸出位置做差，其結果為29 m，就東向定位精度而言，采用強跟蹤五階CKF濾波算法，其定位精度提高了31.1%。

5 結束語

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